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ABSTRACT 


The major problem addressed by this research is the large and/or expensive equipment 
required by a conventional navigation system to accurately determine the position of an 
Autonomous Underwater Vehicle (AUV) during all phases of an underwater search or 
mapping mission. 

The approach taken was to prototype an integrated navigation system which combines 
Global Positioning System (GPS)and Inertial Measurement Unit (IMU), waterspeed and 
heading information using Kalman filtering techniques. Actual implementation was 
preceded by a computer simulation to test where the unit would fit into a larger hardware 
and software hierarchy of an AUV. The system was then evaluated in experiments which 
began with land based cart tests and progressed to open water trials where the unit was 
placed in a towed body behind a boat and alternately submerged and surfaced to provide 
periodic GPS updates to the Inertial Navigation System (INS). 

Test results and qualitative error estimates indicate that submerged navigation 
accuracy comparable to that of differential GPS may be attainable for periods of 30 seconds 


or more with low cost components of a small physical size. 
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I. INTRODUCTION 


A. BACKGROUND 


An Autonomous Underwater Vehicle (AUV) can be capable of numerous missions 
both overt and clandestine. Such vehicles have been used for inspection, mine 
countermeasures, survey, observation, etc. [Yuh 95]. Recent research trends in underwater 
robotics have emphasized minimizing the need for human interaction by increasing the 
autonomy of such vehicles. 

The NPS “Phoenix” AUV is an experimental vehicle designed primarily for research 
in support of shallow-water mine countermeasures and coastal environmental monitoring 
[Healey 93, 95]. In [Kwak 93], an approach is described for determining the position of 
submerged detected objects by executing a “pop-up” maneuver to obtain a GPS fix, and 
then extrapolating this fix backwards to the submerged object location using recorded 
inertial data. As explained in [Kwak 93], navigation accuracy during such a surfacing 
maneuver is strongly enhanced by the use of accurate depth information available from 
low-cost pressure cells. However, this form of “aided” inertial navigation [Brown 92], is 
not applicable to a surfaced AUV. Of course, inertial navigation is not needed in 
circumstances where continuous reliable reception of GPS satellite signals is possible. 
However, this does not apply to AUVs, unless perhaps they are fitted with a mast to extend 
a GPS antenna above the effects of wave action. Such a mast is not an attractive option for 
military operations, and in any event may be mechanically difficult. 

Recognizing the problem of intermittent GPS satellite tracking for surfaced (or 
cruising near the surface) AUV navigation, an experimental system has been designed 
which uses a low-cost strapped-down inertial measurement unit (IMU) to enable inertial 
navigation between GPS fixes. This IMU also is appropriate to pop-up navigation, so 
finding a means of navigating near the surface provides a complete solution to the overall 


navigation problem associated with transiting an AUV to a shallow water work site, 








recording the position of detected submerged objects, and then returning to a recovery site 
where stored mission data can be uploaded. 

Many of the missions of the Phoenix class of vehicles can be separated into two distinct 
phases: transit and search. After being launched from an aircraft, submarine or surface 
vessel such an AUV would conduct the transit phase of the mission in order to arrive at the 
search area. After the search phase, the AUV would transit back to a recovery position. 
Neither of these transit phases require as high a degree of navigation accuracy as the search 
phase. Once established in the mission area, the Phoenix would enter the search phase 
which might include missions such as minehunting, mapping, or environmental data 
collection. Typically, the search phase would require more precise navigation which could 
be provided by more frequent GPS fixes or by using Differential GPS (DGPS) or post- 
processing, if available. Both mission phases may involve waypoint steering and (AJ) 
artificial intelligence applications such as obstacle avoidance. 

One of the most important and difficult aspects of an AUV mission is navigation. It is 
important that the navigation system be robust if the AUV is to be capable of a wide variety 
of missions. In order to achieve such robustness, the AUV should be capable of navigating 
with the Global Positioning System (GPS) and/or an Inertial Navigation System (INS). The 
GPS is capable of highly accurate positioning when the AUV is surfaced, while an INS can 
be used for submerged navigation. In order to ensure accurate navigation for the various 
missions, the GPS and INS components can be combined. A favorable analysis of this type 
of navigation system was conducted in [McKeon 92]. The hardware and software 
architecture required for a typical mapping scenario was evaluated in [Norton 94]. 

In order to implement intermediate testing of a prototype navigation subsystem prior 
to installation into the Phoenix, the navigation system reported in this thesis was broken 
into two subsystems in which a minimum number of components were placed in a towable 
device (towfish) which can be commanded to submerge and surface. The remainder of the 


components were placed in the towing vessel. This results in a smaller towfish, with 














reduced power requirements, and also allows for human monitoring and interaction during 


the course of an experiment. 


B. RESEARCH QUESTIONS 

This thesis will examine the following research areas: 

- Evaluate the hardware and software architecture for the GPS/INS installation in the 
Naval Postgraduate School (NPS) Phoenix AUV. 

- Evaluate the feasibility of an AUV navigating from point to point using GPS/INS 
while conducting open-ocean transit. 

- Develop the software interface which will communicate between the embedded GPS/ 
INS and the Phoenix controlling programs. 


- Evaluate the capability of the Phoenix to obtain accurate navigation fixes by utilizing 


Differential GPS (DGPS). 


C. SCOPE, LIMITATIONS AND ASSUMPTIONS 


This thesis reports the findings of the fourth year of research in an ongoing research 
project. The scope of this thesis is to investigate the feasibility of an AUV navigating from 
point to point using a combination of GPS/INS. Also, the scope includes developing a 
simulation written in LISP which implements a GPS/INS navigation system and a Kalman 
filter. The requirements for a Small Autonomous Underwater Vehicle (AUV) Navigation 
System (SANS) described by [Kwak 93] which impact this project are: 

- Low power consumption. Operation from an external battery pack for 24 hours is 
desirable. 

- Limited exposure time. The amount of time that the GPS antenna is exposed in the 
search phase should be as short as possible. Up to 30 seconds of exposure is allowed, but 


time between exposures should be maximized. 





- Maintain clandestine operation. The GPS antenna should present a very small cross 
section when exposed and ought not extend more than a few inches above the surface of 
the water. 

- Maximize accuracy. During the search phase of the mission, system accuracy of 10 
meters rms or better is required with postprocessing, both submerged and surfaced. 

- Total volume not to exceed 120 cubic inches. Elongated, streamlined packaging 1s 
preferred. 

- For the purposes of this research, DGPS will be used as ground truth data (without 
postprocessing) for determining appropriate Kalman filter gains. However, most real- 
world scenarios will only utilize the noncorrected GPS signal for real-time mission 


navigation and may require further tuning of Kalman filter coefficients. 


D. ORGANIZATION OF THESIS 


The purpose of this thesis is to present the interim development of a system meeting 
the mission requirements of the SANS. The term AUV is understood to include any small 
underwater vehicle (including human divers) which can easily carry such a compact device. 
This thesis provides an evaluation of the hardware and software used to provide accurate 
navigation for the NPS AUV. The major thrust of the thesis is twofold: first, evaluating 
the experimental results of a GPS/INS integrated unit (towfish) which was towed behind a 
boat in both submerged and surfaced states; second, developing a LISP computer 
simulation which implements a GPS/INS integrated system which also utilizes a Kalman 
filtering technique. 

Chapter II reviews the previous work on this project as well as previous work on GPS 
navigation and AUV submerged navigation. 

Chapter III is a detailed problem statement which includes the mission requirements, 
and the problems related to GPS navigation, submerged navigation, and the LISP 


navigation simulation. 











Chapter IV is a detailed description of the hardware currently in use for this portion of 
the project. A description of each of the components used for the towfish experiment is 
included. 

Chapter V is a detailed description of the software used for this portion of the project. 
The chapter includes a description of the C++ code required for the towfish experiment. 
This description provides an explanation of the class and object hierarchy used, as well as 
an outline of the functions of the major functions. The Kalman filter of the INS is also 
discussed here. 

Chapter VI discusses the LISP simulation. It explains how the objects fit into Rational 
Behavior Model hierarchy and highlights the purpose each class serves in the simulation. 
In describes in some detail the mathematics behind the motion simulation. A sample 
display of the AUV tracking from point to point is included here. 

Chapter VII is a description of the experiment design and analysis of the experimental 
results. This chapter covers the methodology of the design and implementation of the 
towfish experiment as well as the push cart and bench tests. 

Chapter VIII presents the conclusions and recommendations of the hardware and 
software testing and provides recommendations for future research. 

Throughout the research conducted as part of this thesis, tasks were divided between 
software and hardware. Eric Bachmann was principally responsible for the former. David 
Gay was principally responsible for the later. Initial authorship of chapters I, II, [V, and VI 
belongs to David Gay. Eric Bachmann authored the first drafts of Chapters II, V, VII, and 
VII. 














Il. SURVEY OF RELATED WORK 


A. INTRODUCTION 


Autonomous Underwater Vehicles (AUVs) have the potential to be used in an efficient 
and cost effective manner in a variety of missions involving military and non-military 
applications. One of the most important aspects of the AUV mission is the ability to 
navigate accurately. Many possible missions, (such as minehunting), require a high degree 
of navigation accuracy. This chapter will discuss some of the solutions for navigating the 
AUV. Additionally, computer simulations of robot kinematics and dynamics can be used 
to simulate the actions of the AUV, thus helping to establish the requirements of the 
navigation system. 

In general there are two categories of navigation systems: those that are based on 
external signals and those that are based on sensors. External-signal-based navigation 
systems such as, Loran, Omega, and Global Positioning System (GPS) are only able to 
determine position while the signal receiver is exposed to the signal. Loran and Omega are 
relatively inaccurate compared to GPS. While Loran covers almost the entire northern 
hemisphere, it has almost no coverage in the southern hemisphere [Bowditch 84]. GPS on 
the other hand is capable of world-wide coverage with a high degree of navigational 
accuracy. 

Sensor-based navigation can be implemented as a self-contained unit which can be 
composed of various types of equipment such as inertial measuring units (IMUs), acoustic 
transponders, or geophysical map comparison. All sensors are subject to some amount of 
error and on long AUV missions this error may not allow for the accuracy required by some 
missions. Each of these components has its disadvantages. Acoustic beacons must be pre- 
deployed at precisely known locations and may require costly maintenance. Geophysical 
map interrogation requires a precise bottom contour map previously stored in the AUV’s 
computer. IMU-based navigation is prone to sensor drift, which if left uncorrected, can 


become very large. 





B. GPS NAVIGATION 





The Navigation Satellite Timing and Ranging (NAVSTAR) Global Positioning 
System (GPS) is a space-based radio positioning, navigation and time-transfer system 
sponsored by the U.S. Department of Defense (DoD). It was originally intended to provide 
the military with precise navigation and timing capabilities [Parkinson 80]. The system is 


designed to provide 24-hour, all-weather navigation by providing total earth coverage 


using 24 satellites in 22,200 km orbits that are inclined 55°, with 12 hour periods. The 
satellites broadcast two L-band frequencies: L1 (1575.4 MHz) and L2 (1227.6 MHz). 
Navigation and system data, predicted satellite position (ephemeris) information, 
atmospheric propagation correction data, satellite clock error information, and satellite 
health data are all superimposed on these two carrier frequencies [Logsdon 92, Wooden 
85}. 

There are two different navigation services available from the GPS satellites 
depending on the type of receiver being used: the Standard Positioning Service (SPS) and 
the Precise Positioning Service (PPS). The SPS is achieved by receiving the L1 carrier 
signal which is broadcast with an intentional inaccuracy called Selective Availability (SA). 
SA limits world-wide navigation to 100 m horizontal accuracy with a 95% confidence level 
[Logsdon 92]. The PPS is limited to US and allied military, and specific non-military uses 
that are in the national interest. Access to PPS is restricted by use of special cryptographic 
equipment. PPS provides the highest stand alone accuracy: 15 m Spherical Error Probable 
(SEP), a velocity accuracy of 0.1 m/sec, and a timing accuracy of better than 100 
nanoseconds [Logsdon 92, Wooden 85]. 

In order to take full advantage of GPS precision without having access to 
cryptographic equipment, the civilian market had to determine a way to improve the 
accuracy of the SPS. The two most common ways to work around the inaccuracies of the 
SPS are real-time Differential GPS (DGPS) and post-processing DGPS. The idea behind 
DGPS is to survey a receiver at a stationary site, allow the stationary site to determine the 


difference between its actual position and its GPS position, and broadcast the pseudorange 








corrections to any DGPS capable receivers. Real-time differential processing can reduce 
the typical 100 m accuracy of the SPS to 2-4 m regardless of the status of SA [Logsdon 92]. 
In the case of post-processing, it is possible to have the AUV record the raw PPS or SPS 
GPS information for later comparison to a known geographical site. Precise post- 
processing procedures can be used to reconstruct extremely accurate positioning 


information, typically in the submeter range. Table 1 shows a comparison of expected GPS 


POSITIONING SERVICE PPS (m) SPS (m) 
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TABLE 1: Expected RMS GPS accuracy levels [Logsdon 92] 


accuracies. 












As the GPS technology has matured, the size and cost of GPS receivers has decreased 
drastically. Not only is miniaturization improving, but GPS receivers have maintained or 
increased in performance capability. Since as early as 1992, the GPS industry has been able 
to produce receivers that are essentially a single printed circuit board. [Souen 92] reports 
that the Furuno GPS receiver module LGN-72 is an eight-channel receiver which is a single 
printed circuit board measuring 100 mm x 70 mm x 20 mm and requiring only 2 W of 
power. 

Given the level of miniaturization and performance along with the excellent accuracy, 
GPS is an obvious choice for AUV navigation. One manner of using GPS to locate an AUV 
is to place buoys with GPS receivers at appropriate locations. These buoys would translate 
the GPS signal and retransmit an underwater acoustic signal. The AUV would determine 
its position via ranging and position fixes to the buoys. [Youngberg 91] suggests that the 
GPS antenna, receiver, processing and control subsystem, acoustic transmitter, battery 
power, and homing beacon could all be contained in a buoy measuring 123 mm diameter x 


910 mm long and weighing 5-15 kg. A simulation which showed the feasibility of this 





approach is presented in [Leu 93]. The simulation consisted of several sonobuoys spaced 
one kilometer apart. Due to uncertainties in buoy position caused by wave action and 
variations in altitude, the study proposed the use of Kalman filtering techniques to combine 
the outputs of an accelerometer and DGPS to enhance accuracy. Each GPS buoy would 
essentially act asa GPS satellite and broadcast its position via spread spectrum signals used 
by the AUV for ranging. This technique would significantly reduce the requirement to 
predeploy a surveyed transponder field. 

Another possible use for using GPS to determine the AUV’s position is to physically 
mount the GPS antenna and receiver onboard the AUV. One major concern would be that 
the GPS receiver would be unable to acquire satellites in a timely manner suitable to the 
mission due to splash effects on the antenna. [Norton 94] describes both static and dynamic 
test results which show that a submersible system is able to meet the accuracy and time 


requirements of the mission while being splashed by wave wash. 


C. AUV SUBMERGED NAVIGATION 


There are many techniques available for submerged navigation, including dead 
reckoning, inertial, electromagnetic and acoustic navigation. With acoustic navigation, 
time of arrival and direction of propagation of acoustic waves are the two principal 
measurements made. A wide variety of acoustic navigation systems have been developed 
for underwater vehicle use. They are typically divided into long, short, and ultrashort 
baseline systems (LBL, SBL, and USBL). All involve the use of an array of acoustic 
beacons or receivers whose positions must be known to an accuracy better than the desired 
vehicle localization accuracy [Tuohy 93]. Unfortunately, most acoustic navigation systems 
require major expeditions for their accurate set-up and periodic maintenance. This makes 
them expensive and in many ways reduces the level of autonomy achievable by an AUV. 
Also, acoustic navigation methods are affected by changes in the speed of sound in the 
ocean and suffer from refraction and multipath propagation problems in restricted shallow 


water coastal and ice-covered areas [Tuohy 93]. 
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There are various other ways of determining a vehicle’s velocity and position while 
submerged without the aid of external signals. An AUV could use Doppler sonar or side- 
scanning sonar to determine velocity. Charge Coupled Device (CCD) cameras, laser 
scanning or variations in the earth’s magnetic field can also aid in determining position 
[Bergem 93]. Position could also be estimated by the double integration of acceleration as 
sensed by an Inertial Navigation System (INS). 

Unless an AUV has access to acoustic information, it will not be able to refer to 
external signals while submerged. If this 1s the case, then the system must rely on some sort 
of dead reckoning. Dead reckoning is a form of navigation where position is determined by 
integrating estimated velocity over a time interval. Modern dead reckoning systems 
typically use magnetic or gyroscopic heading sensors and a bottom or water-locked 
velocity sensor [Grose 92]. The main problem is that the presence of an ocean current will 
add a velocity component to the vehicle which is not detected by the speed log. In the 
vicinity of the shore, ocean currents can exceed two knots [Tuohy 93]. Using dead 
reckoning with currents which are relatively large in relation to the typical 4-6 knot speed 
of an AUV can produce extremely inaccurate results [Tuohy 93]. 

Inertial navigation is basically a complex method of dead reckoning. In its purest form 
it involves no outside references to fix position. All position data is calculated based on a 
known starting point. An inertial navigation system continuously measures three mutually 
orthogonal acceleration components using various types of accelerometers. These 
measurements are taken in increments and multiplied by elapsed time in order to determine 
the instantaneous velocity. The three-dimensional change in position can then be 
determined by multiplying the velocity by an appropriate time increment. 

There are many techniques for measuring accelerations and angular rates. These 
include using ring laser and fiber optic gyros, rotating mass gyros, vibratory rate sensors 
and high performance Inertial Measuring Units (IMUs). The inertial grade IMUs typically 
contain three angular rate sensors, three precision linear accelerometers and a three-axis 


magnetometer. The acceleration measurements required by an Inertial Navigation System 
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(INS) can be made by several types of Inertial Measurement Units. These can be divided 
into two fundamental categories: gimbaled and strapdown. In a strapdown unit, three 
mutually orthogonal accelerometers and three angular rate sensors are mounted parallel to 
the body axes of the vehicle. Changes in vehicle attitude, position and velocity can then be 
continuously measured. Strapdown systems are smaller and simpler than gimbaled 
systems, but necessitate much larger computational loads [Logsdon 92]. All of these 
sensors are subject to drift errors which relentlessly increase with time. High quality 
sensors are subject to less drift but can cost up to $100,000 [Tuohy 93] making them 
unattractive for small AUVs. 

[McKeon 92] proposes a combination of GPS and INS to allow an AUV to determine 
position information. While submerged, the AUV is uses a low-cost inertial navigation 
system. However, when on the surface the vehicle has access to GPS information. GPS/INS 
information could be combined with a Kalman filter techniques to reduce the errors during 
the next dive sequence as simulated in [Nagengast 92] and demonstrated in [McGhee 95]. 


A more thorough discussion of Kalman filtering techniques can be found in [Brown 92]. 


D. ROBOT KINEMATICS AND DYNAMICS SIMULATION 


The motion of rigid bodies, as described in physics and engineering, can be used to 
simulate movement by using robot kinematics and robot dynamics. Robot kinematics is a 
systematic approach of using vector and matrix algebra to represent the spatial geometry of 
an object with respect to a fixed frame of reference, all as a function of time. Robot 
dynamics uses the mathematical equations and physical laws describing motion such as 
Newton-Euler equations to represent motion from applied forces and moments [Fu 87]. 

Using robot kinematics and dynamics to model real-world motion of an AUV ina 
computer simulation is an extremely powerful tool. Simulation allows a system designer to 
establish the requirements of the system prior to proceeding to expensive fabrication and 
testing in a possible unfriendly environment. [Davidson 93] provides an explanation of the 


kinematics and graphical computer simulation code for an underwater walking machine 
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written in Common Lisp Object System (CLOS) and C**. The object-oriented 
programming approach used allows the programmer to easily make adjustments and 
modifications to individual components in complex system designs. 

[Norton 94] describes a computer simulation of an AUV navigating with the SANS. 
The simulation was used in order to help determine the errors created by the various 
sensors. The code was written in CLOS and allowed the AUV and SANS to be represented 
as objects and classes were are linked together to create an entire system which was able to 
move as a single unit. Using robot kinematics and dynamics based on Newton-Euler- 
equations of motion, the system was able to simulate an actual mission. 

Observing, communicating with, and testing underwater robots is difficult due to their 
operations in remote and hazardous environments. A great number of robotics-related 
simulations have been produced, but few involve mobile robots. These simulations are 
typically approached in a piecemeal and fragmented fashion. Thus simulation results 
remain susceptible to failure when deployed in the real world due to the untested 
complexity of unforgiving environments [Brutzman 94]. There is no safe and complete 
“practice” environment for AUVs, since test tanks cannot reproduce the variability of 
critical parameters found in the ocean and since any in-water failure may lead to vehicle 
damage or loss due to flooding. [Brutzman 94] develops a virtual world using 3D real-time 
graphics designed from the perspective of the robot, enabling realistic AU V evaluation and 
testing in the laboratory. A networked architecture enables multiple world components to 
operate collectively in real time, and also permits world-wide observation and 
collaboration with other scientists interested in the robot and virtual worlds. The real-time 
six-degree-of-freedom hydrodynamics model used in the virtual world provides an 
excellent opportunity for further testing of robot navigation algorithms in the presence of 


buoyancy effects and wave action. 
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E. SUMMARY 


The previous survey has shown that there are many ways to overcome the challenges 
associated with AUV navigation. The choices range from simple dead reckoning to systems 
which use acoustic information from floating or stationary transponders to complex 
systems which use sophisticated IMUs and GPS receivers combined with Kalman filtering 
techniques. In order to reduce the hazards associated with the design of a physical system, 
it is possible to attempt to model the proposed AUV using robot kinematics and robot 
dynamics in a computer simulation. This simulation can be used to establish system 
requirements prior to placing the physical system in a dangerous real-world environment. 
The final choice of a navigation system is largely dependent on the mission profile which 


may have different requirements for navigation accuracy at different stages. 
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Il. DETAILED PROBLEM STATEMENT 


A. INTRODUCTION 


There are many possible types of missions which can be carried out by an AUV. Many 
of these missions involve some form of underwater mapping or search for objects of 
interest. This implies that the locations of many of the objects encountered by the AUV 
need to be accurately known once the mission has been completed. Given this requirement, 
the ability of the AUV to accurately determine its position (either in real time or following 
the mission during postprocessing) is a key requirement. The exact nature of the mission 
(whether overt or clandestine) places limitations upon how this determination can be made. 
These limitations are manifested in both the maximum time the AUV can spend on the 
surface and the types of external navigational assistance available to it. In either case, some 
combination of GPS and inertial navigation may be able to provide the required accuracy. 

The general profile of an AUV search or mapping mission can be divided into phases 
consisting of two types. The first type can be termed a transit phase. During this phase the 
AUV would navigate relatively great distances between search areas, transit from a launch 
point to an initial search area or transit from the final search area to a recovery point. These 
mission phases are be characterized by a need for only moderately accurate navigation 
information. For the most part this could be accomplished using periodic fixes together 
with dead reckoning. The second type of mission phase is search. During a search phase 
the AUV would attempt to locate objects of interest and record their locations. The 
accuracy of navigation required during the search phase is much greater, whether 
accomplished in real time or during mission postprocessing. Navigation during this phase 
must be continuously accurate, not just during short discrete time periods following GPS 
fixes. 

[Norton 94] demonstrates the feasibility of using GPS to attain accurate positional 
fixes on the surface by using differential postprocessing. Whether due to errors or 


intermittent fixes, GPS positional information can be described as accurate only in the long 
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term. In comparison, inertial data is accurate in the short term but tends to drift over time. 
The data of the two systems can therefore be used to complement each other. The research 
of this thesis combines real-time differential corrected GPS data with the measurements 
taken from a low-cost Inertial Measurement Unit (IMU), all using Kalman filtering 
techniques. This provides a continuously accurate estimate of position in real time as 
required during the search phases of an AUV mission. In this refined configuration, the 
methods used will meet the requirements outlined for the SANS baseline system described 


in [Kwak 93}. 


B. GPS NAVIGATION 


The Navstar Global Positioning System is a satellite-based navigation system. It 
provides continuous world-wide navigation information to an unlimited number of units 
equipped with receivers capable of processing the signals being broadcast by the satellites. 
There are two different levels of navigation service provided by the system: the Standard 
Positioning System (SPS) and the Precise Positioning System (PPS) [Logsdon 92]. The 
PPS utilizes an encrypted P-code (Precision code) which is reserved for high-precision 
military users. This code restricts the most accurate navigation information of the Global 
Position System to US and allied military and specific US non-military users. PPS provides 
a stand-alone accuracy of 15 m SEP (Spherical Error Probable), a velocity accuracy of 0.1 
m/sec, and a timing accuracy of better than 100 nanoseconds [Van Dierendonck 80, 
Wooden 85]. In most cases it is not be desirable to provide an unmonitored AUV with the 
cryptographic keys needed to obtain PPS due to the fallible nature of any safe guarding 
system for the keys and the potential for compromise. 

SPS information includes an intentional inaccuracy which is introduced into the 
satellite broadcast signal through a process called Selective Availability (SA). This limits 
SPS to a 100 m horizontal accuracy with a 95% confidence level. Differential GPS (DGPS) 
is a method which allows highly accurate information to be obtained from GPS without the 


_ cryptographic equipment required for access to the P-code of PPS. DGPS entails placing 
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one receiver at a known location, determining local satellite range errors, and broadcasting 
error corrections to nearby GPS users. Differential processing can be done in real-time or 
during mission postprocessing. This procedure improves GPS accuracy to 2-4 m regardless 
of whether SA is utilized [Logsdon 92]. 

SPS could be used to adequately perform both the transit and search phases of an AUV 
mission. During the transit phases, non-differential SPS and a magnetic compass would 
provide the primary source of navigation data. In order to utilize GPS as a meaningful 
correction to a low-cost INS system, periods between fixes during the transit phase must 
not exceed the time in which an AUV could travel a distance greater than the horizontal 
accuracy of SPS (100m). The mapping phases of an AUV mission would require the 
vehicle to maintain more accurate navigational picture both submerged and on the surface. 
This would necessitate the use of periodic differential corrected GPS information in order 
to keep the INS system accurate while submerged. This differential correction could be 
provided in real-time during overt missions along friendly shores providing a DGPS 
reference signal is provided or during mission postprocessing following a clandestine 


mission. 


C. INERTIAL NAVIGATION 


Inertial navigation is basically a complex method of dead reckoning. In its purest form 
it involves no outside references to fix position. Ail position data is calculated relative to a 
known starting point. An inertial navigation system (INS) continuously measures three 
mutually orthogonal acceleration components using accelerometers. These measurements 
are taken in short time increments and multiplied by elapsed time in order to determine an 
estimate of instantaneous velocity. The three-dimensional change in position can then be 
determined by integrating respective velocities over time. 

The primary drawback of any INS is the tendency for small sensor drift rates to 
accumulate errors over time. Without outside references for correction, these errors grow 


relentlessly and eventually lead to large errors in the estimated position. Highly accurate 
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inertial navigation systems can be constructed but they are large, costly, and complex 
[Tuohy 93]. Size alone makes them unacceptable candidates for the SANS. In order to meet 
SANS physical requirements, a low-cost INS can be integrated with GPS. GPS will provide 
the INS with the periodic position fixes necessary to correct slowly building INS errors. 
The acceleration measurements required by an INS can be made by several types of 
Inertial Measurement Units (IMUs). These can be divided into two fundamental categories: 
gimbaled and strapdown. Due to their large size and power requirements most gimbaled 
systems are not suitable for the SANS. In a strapdown unit, three mutually orthogonal 
accelerometers and three angular rate sensors are mounted parallel to the three body axes 
of the vehicle. Changes in linear and rotational velocities are continuously measured. 
Strapdown systems are smaller and simpler than gimbaled systems, but necessitate much 


larger computational capabilities [Logsdon 92]. 


D. INTEGRATED INS/GPS NAVIGATION 

Integration of INS and GPS into a single system can produce continuously accurate 
navigational information even when using relatively low-cost components. This integration 
not only allows periodic reinitialization of the INS to correct accumulated errors but can 
also (with the aid of Kalman filtering techniques) improve the performance of the INS 
between fixes. Filtering the acceleration data with additional sensor information such as 
water speed and heading will further improve the quality of the integrated system. Overall, 
an integrated system will provide improved reliability, smaller navigation errors and 
superior survivability [Logsdon 92]. 

Kalman filtering is a method of combining all available sensor data regardless of their 
precision to estimate the current posture of a vehicle [Cox 90]. The filter is actually a data- 
processing algorithm which minimizes the error of this estimate statistically using currently 
available sensor data and prior knowledge of system characteristics. Each piece of data is 
weighted based upon the expected accuracy of the measurement it represents. In a 


complementary filter, low- frequency data which is trusted over the long term and high- 
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frequency data which is trusted only in the short term are used to “complement” each other 
providing a much better estimate than either can alone [Brown 92]. 

For this research the low-frequency data of the accelerometers and compass is 
combined with high-frequency angular rate and heading information using this 
complementary filter technique. Intermediate results are again filtered using high- 
frequency water-speed data. GPS data is used to reinitialize the system each time a fix is 


obtained and develop an error bias to correct the system between fixes. 


E. NAVIGATION SIMULATION 


The SANS proposed for use aboard the Phoenix has been simulated in order to 
simplify the process of determining the errors created by the sensors. This simulation is 
written using Common LISP Object System (CLOS), an object-oriented programming 
language. This allows the hardware and software of the SANS and AUV to be represented 
as objects and classes that are combined to create an entire system. The simulation further 
gives the AUV and its various components the physical properties of a rigid body. Other 
objects in the simulation represent the various software objects needed to interface with the 
hardware and a tactical level navigator as described in [Byrnes 93]. The navigator object 
maoniiar the position of the AUV and makes the required inputs to the AUV to navigate 
between a series of waypoints. The AUV and its components then move as required to 
Simulate an actual mission. Using kinematics and dynamics based on Newton-Euler 
equations [Fu 87], the final Cartesian coordinates for the system can be continuously 
determined. 

Within the simulation, each sensor software module has a slot containing the 
measurements that would be obtained by that sensor in the actual navigation system. This 
simulation allows changing the parameters of the AUV missions and sampling a number of 
missions. By running a new simulation, an estimate of the resulting error can quickly and 
easily be determined. A 2D graphical representation of the AUV is included as a way of 


visually to demonstrate the approximate error of the system. This simulation can also be 
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tailored to support different types of AUVs using different types of sensors. Simulation 


details are explained in Chapter VII. 


F. SUMMARY 


Many AUV missions could be accomplished using an integrated navigation system 
combining GPS and INS. Similar systems in other applications have been demonstrated to 
have superior GPS signal acquisition and reacquisition performance whenever loss of lock 
occurs, resulting in improved survivability in hostile environments and smaller navigation 
errors. This research continues an ongoing experimental study pertaining to the 
development of such a system and the associated problems. The current system under 
evaluation is of small physical size and relatively low cost. The IMU selected is 
representative and has limited accuracy, so additional water-speed and magnetic heading 
information is required. This means that accelerometers are used mainly to derive low 
frequency attitude information, and are not utilized for velocity or position estimation over 
long periods. 

The availability of differential GPS in open-ocean tests in Monterey Bay will allow the 
experimental choice of navigation filter gains to accurately assess overall system 
performance in a variety of sea states and for various operational scenarios. The research 
goal of this thesis is to produce test results and qualitative error estimates which indicate 


that submerged navigation accuracy comparable to GPS surface navigation is attainable. 
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IV. GPS/INS HARDWARE INTEGRATION 


A. INTRODUCTION 


Figure 1 shows a block diagram of the hardware assembled for at-sea testing of the 
SANS system concept. Figure 2 presents a photograph of the major components of the 
corresponding physical system. The towfish was designed and built by Russ Whalen. The 
10 Hz filter was built and tested by Walter Schubert [Schubert 95]. Comparison of Figure 
1 to the system described in [Kwak 93] reveals a number of differences. First of all, to 
enable experiments using a towfish rather than an AUV, the SANS system has been broken 
into two subsystems in which a minimum number of components have been placed in the 
towfish itself and the remainder are in the towing vessel. This results in a smaller towfish, 
with reduced power requirements and also allows for human monitoring and interaction 
during the course of an experiment. When this version of SANS is integrated into Phoenix 
(or any other AUV), the modems and towfish data logging computer shown on Figure 1 
will no longer be needed, and the computer in the towing vessel will be replaced by the 
AUV onboard navigation computer. 

Other differences relative to [Kwak 93] include replacement of the depth cell used for 
pop-up navigation by a water-speed sensor. This is because depth rate cannot be used for 
water speed estimation during surfaced navigation. Naturally, a complete SANS would 
include both sensors to enable both navigation modes. Additionally, estimation of water 
speed from depth rate by deliberate “porpoising” (periodic excursions in the vertical plane) 
during submerged navigation may be useful. It is expected that this possibility will be 
investigated after SANS is installed in the Phoenix. Another change to the earlier SANS 
concept is the replacement of rotating gyros by miniaturized vibratory angular-rate sensors 
for improved reliability and to eliminate AUV maneuvering limits imposed by rotating 
gyros [Kwak 93]. These sensors are packaged together with three precision linear 


accelerometers in the integrated IMU shown in Figure 2 [Systron-Donner 94]. 
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Figure 1: GPS/INS Hardware Integration 
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Figure 2: SANS and Towfish Components 








During testing of this system, an Intel 386 based laptop PC was used for data 
collection. Processing of test data was accomplished off-line with a Sun 4 workstation. 
When SANS is installed in the Phoenix, an onboard notebook workstation (Sun Voyager 
or similar system) will replace both of these computers for real-time navigation. This 
computer will also perform other mission control functions as described in [Byrnes 93, 
Healey 95] while vehicle control will continue to be accomplished by the current Phoenix 
OS-9 system hosted on a Gespac 68030 computer running execution level software [Healey 


93, 95] [Brutzman 94]. 


B. HARDWARE DESCRIPTION 


1. Data Loggmg Computer 


The Tattletale Model 5F-LCD computer was specifically designed for portable 
embedded applications requiring small size, low cost and high reliability. In order to 
conserve power, the Tattletale is equipped with two different modes of operation: sleep and 
run. The system’s typical current drain is a strong function of the program that it is required 
to run, with the sleep mode only requiring 2.7 mA and a peak current rating of 20 mA. The 
Tattletale is equipped with a 12-bit analog-to-digital (A-D) converter which is capable of 
handling eight analog signals at a maximum sampling rate of 1600 Hz. The system also has 
a 480 KB data storage capacity and a 32 KB Flash EEPROM capacity for user programs 
[Tattletale 94a]. 

The Tattletale was programmed in TxBASIC to multiplex the six outputs from the 
Systron-Donner IMU, the water speed sensor and the compass. The software code 
described in [Schubert 95] was burned into the flash EEPROM and the program repeatedly 
fetched all eight A/D channels, using two bytes per sample, until 128 bytes of data was 
collected. The 128 byte packet was then transmitted via the XMODEM protocol to the 
towing vessel for processing. XMODEM was used due to easy availability on both units 


and since it includes both error correction and flow control. 
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2. Inertial Measuring Unit 


The inertial navigation component of the AUV Phoenix was provided by a Systron- 
Donner Model MP-GCCCQAAB-100 “MotionPak” inertial sensing unit, pictured in 
Figure 3. This self-contained unit provides analog measurements in three orthogonal axes 
of both specific force and angular velocity. It consists of a cluster of three accelerometers 
and three “Gyrochip” angular rate sensors. General specifications are shown in Table 2. 
Accelerometer specifications and angular rate specifications are shown in Table 3 and 


Table 4 respectively. 
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Figure 3: Systron-Donner Inertial Measuring Unit 
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TABLE 2: MotionPak general specification [Systron-Donner 94] 
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TABLE 3: MotionPak accelerometer specifications [Systron-Donner 94] 
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Parameter Units X-axis y-axis 


mV/deg/sec 50.151 49.906 50.242 
Scale Factor Temp. Coefficient %ldeg C 









TABLE 4: MotionPak angular rate sensor specification [Systron-Donner 94] 


3. Other Components 


The GPS receiver used is the Motorola PVT6 receiver [Motorola 93a] which 
incorporates a Differential GPS (DGPS) capability and is able to track up to six satellites 
simultaneously. It can provide position accuracy of better than 25 meters spherical error 
probable (SEP) without Selective Availability (SA) and 100 meters (SEP) with SA on. 
Typical Time-To-First-Fix (TTFF) is 60 seconds with a reacquisition time of less than four 
seconds when the antenna has been obscured for up to 60 seconds [Motorola 93a}. [Norton 
94] demonstrated that under normal operating conditions this receiver is capable of meeting 
the accuracy and time requirements of the SANS project. [Norton 94] also demonstrated 
this unit will perform well when using an antenna that is located on or near the sea surface 
as is necessary during a clandestine mission. 

Each of the six output channels of the IMU are externally filtered by an active analog 


anti-aliasing filter with a bandwidth of 10 Hz. The filter is also used to convert the two- 
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sided IMU analog output to a single-sided signal within the 0 to 5 volt range of the A to D 
converter [Schubert 95]. 

The modems used were Pocket Peripherals Model PM14400FX [Practical 92] 
operating at a 9600 baud data rate. Communication from the towfish to the towing vessel 
was via a 100 ft analog cable using XMODEM protocol. This had the consequence that 128 
byte packets were transmitted at approximately a five Hz rate to the laptop computer in the 
towing vessel. Each of these packets contained eight samples of each of the eight inputs to 
the towfish A to D converter as shown in Figure 7. Thus an average sampling rate of 40 
Hz was achieved for each of these signals. This represents a two times oversampling of the 
10 Hz bandwidth analog signals, thereby ensuring that noise aliasing was not significant in 
any subsequent digital processing. 

As described in [Kwak 93] and [Norton 94], the magnetic compass used is a KVC 
C100. The water speed sensor is a paddlewheel type used for small boat applications 
(Tritech 95] and is essentially a four-pole rotor, single-stator alternator. AC signal 
frequency and voltage are proportional to sensed speed. These, and other system 
components shown in Figure 2, were selected based on proven technology and performed 


well in the SANS environment. 


C. SUMMARY 


The interim SANS design described in [Norton 94] is the basis for the system 
described in this section. The research of this thesis explores replacing the single linear 
accelerometer used in [Norton 94] with a three-axis IMU. The information provided by the 
IMU (filtered with a 10 Hz anti-aliasing filter), coupled with waterspeed and heading 
information, is multiplexed through a 12-bit A -D converter prior to being transmitted to 
the towing vessel via modem. The hardware for this version of the SANS was chosen to 
comply with the requirements set forth in [Kwak 93]. Even though there are many possible 
choices of hardware for each of the components in Figure 2, trade-offs between accuracy, 


size, power requirements, and cost must be considered. As further advances in 
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miniaturization are made, accuracy will continue to increase while price and size decrease, 


thus making it easier to meet the challenges of the SANS baseline requirements. 


2 
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V. SOFTWARE DEVELOPMENT 


A. INTRODUCTION 


The purpose of the SANS software is to utilize IMU, heading, and water-speed 
information to implement an INS, and then integrate this with GPS information into a 
single system which can produce continuously accurate navigational information in real 
time. The INS is implemented using Kalman filtering techniques in which differential GPS 
fixes are treated as ‘error-free data’ allowing periodic reinitialization of the INS to correct 
accumulated errors and develop error biases. Both GPS and INS data are logged in raw 
form for postprocessing. In addition, each position fix is logged to a script file for 


postmission plotting as is attitude information. 


B. SOFTWARE DESCRIPTION 


This implementation continues to use object-oriented paradigms as discussed in 
[Stevens 93]. However, the increased complexity of this experiment, which involved 
navigation data from two separate sources and multiple serial ports, called for a new 
implementation as opposed to adapting previous SANS software. Where previous 
implementations of the SANS [Norton 94, Stevens 93] were done with Ada objects and 
assembly language routines to carry out low-level tasks, this implementation utilizes C++ 
objects to carry out all software operations. Use of a single language implementation 
simplified interfaces between software and hardware objects. The software is designed for 


use on a IBM-compatible personal computer with a 386S8X/33Mhz processor using the 


Borland version 3.1 C** compiler under DOS 5.0. 

Figure 4 shows the SANS software objects and the types of data that are passed from 
one to another. The tasks preformed by the SANS software can be divided into two basic 
categories. The primary tasks are related to calculating the current position. These include 
processing incoming GPS data, IMU data, water-speed, and heading information, and 


integrating all information to obtain a navigational fix. These tasks are performed by the 
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GPS, INS and Navigator software objects respectively. Source code for these objects can 
be found in Appendix A. The secondary tasks involve hardware interfacing, 
communications, data filtering and unit conversion. The source code for these objects can 
be found in Appendix B. These basic but crucial tasks are handled by the Sampler, Buffer 
and Serial Port objects. The main program as illustrated in Figure 4 serves only to drive 


the other objects by continually querying the navigator for position updates. 
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Figure 4: SANS software objects and data flow 








In addition to the software outlined above a small program was implemented in 
TxBasic for execution by the 5F-LCD tattletale. This code is not considered part of the 
SANS software. The Tattletale TxBacsic program multiplexes the six outputs from the 
Systron-Donner IMU, the water speed sensor output and the compass output. Program code 


is included in Appendix E. 


1. Navigator 


The navigator class acts as coordinator of all navigational information. As such it 
determines which source is currently providing the best information, converts various 
position formats from one to another, and instantiates the GPS and INS objects. The 
interface to the object is made up of two public methods. 

The first method of the navigator (initializeNavigator) initializes the navigator, 
preparing it to begin providing the current position upon request. This method obtains an 
initial GPS fix for use as the origin of the grid used by the INS object to specify positions, 
and calls the initialization method of the INS. 

The second navigator method (navPosit) drives both the GPS and INS objects and 
provides the best estimate of the navigator of current position in hours, minutes, seconds 
and milliseconds of latitude and longitude. Each time the method is invoked, it interfaces 
with the GPS and INS objects to determine if none, one, or both have an updated estimate 
of the current position. If no update is available, the navigator returns a negative reply itself 
indicating that it can not provide a position update. If only INS information is available, it 
is converted and returned as the current estimated position. Whenever GPS information is 
available, it overrides the INS estimate of position and is converted and returned as the 
current position. GPS information is also passed to the INS object for reinitialization and 
error estimation purposes. All position information received by the navigator is written to 
a data file in raw format, as well as to a summarizing script file giving the estimated latitude 


and longitude in milliseconds of arc. 
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The navigator deals with three different position formats. GPS positions from the 
Motorola receiver are initially obtained entirely in latitude/longitude milliseconds. INS 
positions are expressed in x-y grid coordinates based upon a navigator-stored origin. The 
positions produced by the navigator itself are expressed in hours, minutes and seconds of 
latitude and longitude. In addition, GPS positions must be converted to grid coordinates 
prior to utilization by the INS. A total of four methods are used to convert from one format 


to another. Figure 5 illustrates uses and conversions of the different position formats. 


USER 








Positions expressed in 
hours, minutes, seconds 
and milliseconds of 
latitude and longitude. 


NAVIGATOR 


Positions expressed 


t : Positions expressed in 
in grid coordinates 


milliseconds of latitude 
and longitude 


Figure 5: Navigation position format utilization. 
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2. GPS 


GPS class objects obtain GPS position messages in the Motorola propriety format 
(@@Ba) and insure their validity prior to providing the information [Motorola 93a]. The 
object also instantiates the gps buffer and serial port objects needed to communicate with 
the GPS receiver via a PC serial port. Interface to the object is provided by a single method 
(gpsPosition). This method checks for the arrival of new messages. If one is available, a 
checksum is performed on it and a determination is made regarding the number of satellites 
in view of the GPS receiver when the received message was sent. If less than three satellites 
were used to produce the fix, it is considered invalid. This requirement is made to insure 


that GPS information will not be used while the SANS is submerged and unreliable. 


3. INS 


The INS class implements the inertial navigation portion of the SANS. It is the most 
complex class in the software. The interface consists of three public methods. Each is 
directly involved in the implementation of a nine-state Kalman filter. The primary method 
(insPosition) combines all sensor information and uses the Kalman filter described in the 
following paragraphs to produce a dead reckoning position estimate. The other methods 
support the primary method by performing special one-time or periodic operations. 
Initialization of the INS is performed by a method (insSetUp) which sets the INS posture 
at the grid coordinate origin, sets an initial heading and speed, and marks the beginning of 
the first integration intervals. The last public method of the class (correctPosition) inputs 
GPS information to reinitialize the INS position while determining a current and error 
correction bias. The INS class instantiates a Sampler object from which it obtains all sensor 
data except for GPS position fixes. It also records attitude information to a script file for 
post-processing and plotting. 

Figure 6 is a data flow diagram for the SANS filter design. The nine state variables 
are the outputs of each of the three integrators and the summer of the Kalman filter and are 


shown in Table 5. The seven continuous-time state components of this filter consist of three 


35 











Euler angles ( ©, 6, ), two horizontal velocities (x, ,y,) and two horizontal positions 
(x, ,y, ). In the actual SANS digital filter implementation, the continuous-time integration 


is approximated by numerical integration, so in this sense the seven “continuous-time” state 
components are “discrete- time” state values. This is necessary due to limitations placed on 


the minimum integration sampling times by the computer and A-D hardware 
characteristics. The two discrete-time state components (x, , y, ) are composed of estimated 


ocean current in the East and North directions. Their discrete nature is due to diving and 
wave action which results in intermittent GPS signal reception. Thus the two “discrete- 
time” states are updated aperiodically as is characteristic of discrete event dynamic systems 
(Ramadage 89]. This being the case, it is difficult to apply Kalman filter theory to obtain 
optimal time-varying values for the gain matrices K; shown in this figure. Instead, constant 
gains were computed initially from bandwidth and steady-state error considerations. 

The continuous state part of Figure 6 shows that the Euler angle and linear velocity 
outputs are fed back to the corresponding integrator inputs. Thus if the gain matrices K,, 
K,, and K; are all diagonal, each of these integrators is in fact a low pass filter for each of 
its inputs. This is done to prevent unlimited growth of state estimates in the presence of 
unmodeled bias errors in state derivative inputs to integrators. Each integrator is also 
furnished with an independent source of low frequency information to correct for long-term 
decay of state estimates resulting from this feedback. This approach is usually referred to 
as “‘complementary”’ filtering, or sometimes as “crossover” filtering. 

The sources of low frequency information include the accelerations sensed by the 
accelerometers (x, , ¥, , 7, ), the magnetic compass readings (‘.) and the water-speed (,, ). 
The accelerometer data in this case is utilized in a manner similar to inclinometer readings. 
This provides information regarding how much of the specific force felt in each axis is due 
to gravity. 

In addition to filtering, the IMU readings require other correction or conversion. The 


specific force readings of the accelerometers are translated into the accelerations ¥, ¥, 7 
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prior to rotation to x, ,¥, ,z,. The angular rate sensor readings are bias corrected prior 
translation into Euler rates in order to correct for rate sensor drift. These biases 
C,,C,, and C,, are continually updated by a low-pass filtering process using the 
equations (5-1), (5-2), and (5-3). 
Cy = K(C,) + A -K) Puncorrected (5-1) 
C, = K(C,) + A -K) Quncorrected (5-2) 


C,=K(C,)+(1-K)r (5-3) 


uncorrected 
They are subtracted from the angular rate sensor readings. Kis a weighting factor, with 
values typically ranging from 0.99 to 0.9999. 

In the SANS filter design, complete confidence is placed in the precision of GPS. 
Matrix K, is therefore set to unity causing the integration of linear velocity (posture) to be 


reinitialized each time a GPS fix is obtained. Further discussion of the software filter design 


and gains can be found in [McGhee 95]. 





Table 5: State variables of the Kalman filter 







4. Sampler 


The Sampler prepares raw IMU, heading and water speed data for use by the INS. This 
preparation includes filtering, unit conversion and time stamping. The Sampler interface 
consists of a single method (getSample) which controls the data formatting and returns a 
formatted sample if valid raw data is available and a negative response otherwise. All 


Sampler methods are dependent on the format of the raw data packets received by the 
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packetBuffer class. Therefore, the sampler will require extensive changes if this format is 
altered. 

The packets processed by the Sampler are received via XMODEM protocol by the 
packetBuffer object. Figure 7 illustrates a typical packet. Each packet contains 132 bytes, 
4 of which are header and 128 of which are data. The first three bytes, in order, consist of 
the ‘“‘start of header” character (SOH 0x01), the packet sequence number and the 
complement of the packet sequence number. These are followed by the data bytes and a one 
byte checksum. The data bytes are divided into eight closely timed samples. Each of these 
samples consists of eight two-byte integers. The first six integers in each sample are IMU 


outputs. The first three are linear accelerations ( x, ¥, 7 ) and the next three are angular rates 


( 6, 6, y ). The seventh integer in each sample is output by the water speed sensor and the 
last integer in each of the eight samples contained in a packet is output from the compass. 
The integers contained in a sample are digital measurements of analog voltages output by 
the SANS sensors. 

The first action taken by the Sampler when a packet is received is to time stamp it. 
Since the time difference between the eight samples contained in a single message packet 
is relatively small, the Sampler object then respectively averages the eight corresponding 
data variables contained in a packet. The averaged measurements which result represent a 
low-pass filtering of the eight samples. Once these eight filtered measurements are obtained 
they are converted from voltages to units which are usable by the INS object (1.e. feet and 
radians). Finally, each of the measurements is checked to ensure that it is within the limits 
of the sensor from which it came. If any values fall outside the capabilities of the sensor 


from which it came, the entire packet is considered invalid and discarded. 


5: Communication Objects 


The Buffer and BufferedSerialPort classes perform the routine but necessary tasks of 


receiving individual characters via serial port and buffering them until they are used to 
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estimate the current position of the SANS. The buffer classes used are actually derived 


from a base buffer class and are specially designed to handle specific types of messages. 


packet | sequence : ; ; : ee 
SOH |sequence] number ar Poe =ay . ae e rae e ee e iis e ue e sate - 
number | complement 





Figure 7: Modem packet format 


The packetBuffer class handles packets of IMU, water speed and heading information. The 
gpsBuffer class handles GPS position messages in the Motorola proprietary format 
(@@Ba) [Motorola 93a]. The BufferedSerialPort class is derived from a base serial port 
class. These two classes set communications parameters, establish new interrupt service 
routine (ISR) vectors and initialize UARTs (Universal Asynchronous Receiver/ 
Transmitter) through which communications are conducted with both the GPS receiver and 
the tow fish. A portBank class controls the multiple instances of serialPort which are open 
simultaneously. This class is not illustrated in Figure 4. 

The packetBuffer class implements a specialized XMODEM protocol used to 
communicate with the Tattletale in the towfish. Unlike a normal XMODEM 
implementation, each packet received is acknowledged as correct to the sender prior to 
inspection of its contents. This eliminates the requirement for the sender to re-transmit 
packets which have become time late. Instead the sender simply transmits an updated 


packet which contains more recent data. Only when an attempt is made to get a complete 
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packet from the buffer are its checksum and sequence numbers inspected. If either is found 
to be invalid the packet is discarded in the expectation that new data will soon be received. 

The gpsBuffer class operates in a manner similar to packetBuffer class. Multiple 
messages are stored in circular buffers, but only the most recent arrival is considered when 
a request 1s made for updated information. As with the packetBuffer, the header and 


checksum are only inspected when a request is made for a message. 


C. SUMMARY 


The SANS software is designed to produce continuously accurate navigational 
information in real time. While submerged, IMU, heading and water-speed information are 
processed by the SANS Inertial Navigation System (INS) to produce a dead reckoning 
position estimation. This is integrated with DGPS information obtained during periodic 
surfacings using Kalman filtering techniques. The DGPS information resets the position of 
the INS. It is also used to generate an apparent current vector to correct future INS position 
estimates. 


The software was implemented using object oriented paradigms. It was written in 


Borland version 3.1, C** for use on a 386SX/33Mhz processor. The primary tasks of the 
software are estimation of current position and communication. The former is handled by 
the Navigator, INS, GPS, and Sampler objects. The later is accomplished by the GPS 
buffer, Packet buffer, Port bank, and Buffered Serial Port objects. The Buffer Serial Port 
and Buffer classes are very general and could be used in a variety of serial port 


communication applications. 
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VI. SYSTEM TESTING 


A. INTRODUCTION 


This chapter presents the test methods and experimental results of static laboratory 
testing and dynamic at-sea testing used to determine the functionality and accuracy of the 
towfish system. Bench testing was performed to ensure the entire system was functional 
prior to at-sea testing. In addition, the system was tested in its entirety by placing it on a 
wheeled cart and pushing it around a measured course. The final at-sea testing was 
performed by towing the towfish behind a boat in Monterey Bay while diving and surfacing 


the towfish at various intervals. 


B. STATIC TESTING 


1. IMU Static Tests 

For cost and availability reasons, a single-sided 12 bit A-D converter [LTC 95] was 
selected for the breadboard SANS shown in Figure 1 and Figure 2. Figure 8 shows a sample 
of typical results obtained from bench testing of an accelerometer and a rate sensor. In this 
test, data from all six channels of the IMU were collected using a Tattletale data logging 


computer [Tattletale 94a]. As can be seen, the acceleration signal fluctuates an average of 


about + one bit. This low level of accelerometer noise is due in part to the fact that each 
of the six output channels of the IMU is externally filtered by an active analog anti-aliasing 
filter with a bandwidth of 10 Hz. This circuit also converts the two-sided IMU analog 
output to a single-sided signal within the 0 to 5 volt range of the A-D converter [Schubert 
95]. 

The x-axis (longitudinal) accelerometer signal shown in Figure 8 was obtained with 
the accelerometer lying on its side on a table. Thus the output should nominally be zero, 
which corresponds to the integer value 2048 for this A-D converter. It can be seen that the 
output is actually centered around 2028, which represents an apparent error of around one 


percent. However, this is not a correct analysis of the error effect. In fact, the output 
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Figure 8: IMU bench test results(83 seconds at 12Hz data rate) 
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mean indicated could be due either to a tilt in the supporting surface or to an amplifier 
imbalance. Either of these effects would be eliminated in a prelaunch alignment and 
initialization phase before conducting a SANS mission. The real significance of Figure 8 is 
that limiting A to D precision to 12 bits seems to be justified, since it can be seen that typical 
IMU sensor noise reaches or exceeds the value of the least significant bit, at least with 10 


Hz anti-aliasing filtering. 


2. GPS Receiver Testing 


As described in [Kwak 93] the Motorola PVT6 GPS receiver possesses generally 
desirable characteristics for the SANS system [Motorola 93a]. As can be seen in Figure 2, 
it is physically quite small. It also possesses a low power sleep mode with the time to first 
fix after one hour of power off typically on the order of 30 seconds [Kwak 93]. This long 
time is needed to acquire ephemeris (orbital) data from new satellites which may have 
come into view since the last GPS fix [Clynch 92]. Accuracy in latitude and longitude has 
been observed in static testing to be around 30 meters rms using the standard positioning 
service (SPS) [Kwak 93]. Figure 9 shows recent bench test results relating to raw GPS 
position estimates obtained with an antenna mounted on top of a five story building at the 
Naval Postgraduate School. Figure 9 also shows the improvement in horizontal position 
error (root sum square of orthogonal horizontal error components) which results from the 
use of differential mode using a Trimble RL base station about three km away at the 
Monterey Bay Aquarium Research Institute (MBARI) building in Pacific Grove. As can be 
seen, differential correction (DGPS) reduces rms radial position error to around 1.4 meters, 
corresponding to rms latitude and longitude errors of approximately 1 meter. These errors 
are generally consistent with the findings of an earlier, more comprehensive study of a 


variety of DGPS systems [Clynch 92]. 
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Figure 9: GPS bench test results(1 hour at 5 second intervals) 
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An important question relating to GPS navigation with the antenna essentially awash, 
as is the case for SANS, is whether or not satellite tracking can be maintained in the 
presence of wave action. To investigate this issue, [Norton 94] conducted bench tests in 
which a plastic pan of seawater submerged the GPS antenna. For satellites at small angles 
from the vertical, it was found that tracking could be maintained reliably for water depths 
of less than five mm [Norton 94]. For satellites nearer the horizon, it was found that the 
slant distance through the water was the limiting factor in determining reception, and that 
this distance also can be up to five mm without seriously affecting reception. Following 
these tests, the GPS antenna was mounted on an earlier simplified towfish [Norton 94], and 
subjected to short dives in which submergence typically lasted from two to five seconds. 
Results of these tests can be found in [Norton 94]. The results show that once satellite 
tracking is lost due to diving, the minimum of three satellites needed for surfaced 
navigation is typically regained in two to five seconds after surfacing. A fix 1s produced at 
that time if the satellite ephemeris data is available in the receiver memory for three or more 
of the satellites being tracked. These results coupled with additional test results reported 
earlier in this chapter, encourage us to believe that wave action will not present a serious 


problem to GPS reception for SANS. 


3. Software Testing 


Extensive static testing of the navigation software was conducted in order to establish 
proper functionality and to aid in determining appropriate Kalman filter gains. The 
Motorola receiver was set to obtain DGPS fixes at intervals of 1, 10, 20, 30, and 60 seconds 
in order to simulate the diving and surfacing of the towfish. For testing purposes the towfish 
was leveled in the roll and pitch attitudes and was placed so that the compass read 180 
degrees magnetic. Figure 10 shows a representative bench test run with the Motorola 
receiver set to 60 seconds between fixes. Analysis of the data shows that, even with a full 
minute between fixes, the Kalman filter appears to be able to significantly reduce the 


position error over a relatively short period of time. The data also demonstrates that the 
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system is statically able to meet the 10 meter accuracy requirement of [Kwak 93] for a large 


majority of the time. 
It should be noted that the above accuracy is accomplished through the development 
of a fictitious “apparent current’, as shown in Figure 6, which compensates for other 


sources of static velocity error in the system. For the results shown in Figure 10, the steady 


state value of this current was around 0.059 knots North and 0.177 knots West (-0.177 


East). 


10 meter navigation 
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Figure 10: Bench test results with 60 seconds 
between DGPS fixes 


Figure 11, Figure 12, and Figure 13 show the roll, pitch, and yaw attitude of the towfish 


during bench tests. The tests results were obtained with the towfish sitting as level as 
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possible with 60 seconds between DGPS fixes. The results show that the Kalman filter bias 
corrections to the rate sensor readings from the IMU were effective in eliminating rate 


sensor drift which is inherent in this type of sensor. 
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Figure 11: Bench test roll attitude 

Further static testing was performed on the IMU sensor while in the towfish. The 
towfish was set up to be as level as possible for a period of two minutes, then the system 
was pitched nose-up to a 14° attitude for a period of two minutes and then returned to a level 
position for the final two minutes. The same test procedure was performed for the roll axis. 
Figure 14 and Figure 15 show the results of the IMU bench tests. 

Figure 14 shows that there was a residual roll angle of approximately -0.5° (left roll). 
Analysis of the data shows that at approximately 120 seconds the roll attitude changed and 
eventually stabilized at a value which reflects a 14° change in attitude. The same effect can 


be seen at 240 seconds. 
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Figure 12: Bench test pitch attitude 
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Figure 13: Bench test heading 
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Figure 15 shows that there was a residual pitch angle of approximately -1.5° (nose 


down). Analysis of the data shows that at approximately 130 seconds the pitch attitude 


changed and eventually stabilized at a value which reflects a 14° change in attitude. The 


same effect can be seen at 245 seconds. 
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Figure 14: Roll attitude transient of 14 degrees 


The large transients observable in Figure 14 and Figure 15 are undesirable and should 
not occur in a properly tuned filter. As of the time of completion of this thesis, however, a 


more appropriate set of input and feedback gains has not been found. This is an important 





area for further research on the SANS which should be undertaken as soon as possible. 





Until such tests are successfully completed, the possibility of a programming error in the 


SANS code must also be considered. 
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Figure 15: Pitch attitude transient of 14 degrees 


C. CART TESTING 


In order to ensure that the entire system was functional and that the software was 
sufficiently robust prior to proceeding to at-sea testing, it was necessary to assemble the 
entire system as it would be configured on the boat. This was accomplished by placing the 
system on a two-tiered wheeled cart as depicted in Figure 16. The cart and towfish system 
were then pushed around a surveyed course. Diving and surfacing of the towfish was 
simulated by setting the Motorola GPS control software to obtain a GPS fix at specific 
intervals. Typical intervals were 1, 10, 20, and 30 seconds between GPS fixes. Between 
each of these GPS fixes, the IMU was estimating and tracking position. As each subsequent 
GPS fix was obtained, the INS was reset to the DGPS position and the Kalman filter then 
used the fix to determine new biases. Typical results of this type of testing are shown in 
Figure 17. Another reason for performing the cart test prior to at-sea testing was to obtain 


initial data to aid in estimating values for the Kalman filter gains. Analysis of the data 
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showed that compass heading was reliable and should have a relatively large gain. The 
same reasoning applied to the water speed sensor. After several iterations it was determined 


that K,, K5, K3, and Ky should be set to 0.1, 0.6, 0.5, and 1.0 respectively. The fact that K4 


is 1.0 implies that the DGPS fixes were used as truth data to reset the INS. 
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Figure 16: System configuration for cart testing 


During the cart test no attempt was made to level the towfish and any results which 
show stabilized values other than zero can be attributed to this fact. Typical roll, pitch, and 
heading results are presented in Figure 18, Figure 19, and Figure 20 respectively. When the 
towfish program was started it was allowed 30 seconds to initialize prior to moving the cart. 
As the cart arrived at point 2 and point 3 the program was once again allowed 30 seconds 
to stabilize prior to continuing to the next point. Proceeding back to point 1 through point 


3 was performed without stopping to stabilize the plot. As can be seen in Figure 18 there. 
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Figure 18: Roll attitude for cart test 
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Figure 19: Pitch attitude for cart test 
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Figure 20: Heading for cart test 


was a residual roll angle of approximately 2.5° which can be attributed to not leveling the 


towfish prior to the experiment. The same type of effect can be seen in Figure 19 which 


shows a residual pitch angle of approximately 1.1°. Both figures show that the system 
appears to be reacting to the movement of the cart as it is pushed around the course. The 
figures also show that the system appears to working correctly since the roll and pitch 
values tend toward the residual values as soon as the cart is at rest. 

Figure 20 shows the heading results from the cart testing. After the program was 
initialized, the cart was pushed in the direction of point 2. The data shows that the cart was 
turned approximately 90 degrees at each point which is a good correlation to the surveyed 
course shown in Figure 17. At point 3 the cart was turned in a direction which passed 
through north which produces the branch cut shown in the figure. Another factor which 
shows that the system appears to be working correctly is that after four turns the final 


heading stabilizes at the same value as the initialization heading. 
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D. AT-SEA TESTING 


An at-sea test procedure was developed after establishing that the proposed mission 
requirements were feasible based on the results of the simulation, bench tests, and cart tests. 
The purpose of the at-sea test was to further demonstrate the ability of the GPS receiver to 
track satellites and obtain DGPS position information while the antenna was subjected to 
wave wash-over. The test was also used to verify the initial concepts of the Kalman filter 
techniques, as well as to obtain raw data for post-processing to be used in optimizing the 
Kalman filter gains. 

The first step was to build a test vehicle that was able to adequately simulate the actions 
of an AUV in the performance of its mission. The test vehicle was required to submerge to 
a depth which was deep enough to obscure the antenna from satellite reception. Also, while 
on the surface the test vehicle had to ride high enough to reduce the susceptibility of the 
GPS antenna to wave effects. The vehicle also had to have the capability to be commanded 
to surface and submerge. The final version of the test vehicle is shown in Figure 2 and 
Figure 16. The vehicle was designed to be towed behind a boat with diving and surfacing 
controlled by pulling on control lines which moved the front set of elevators. This vehicle 
was designed and constructed by R. H. Whalen [Schubert 95]. 

The test was performed in the Monterey Bay in light seas with swells of three to four 
feet. Figure 21 shows the towfish during at-sea tests. The flat Motorola antenna that was 
supplied with the GPS receiver was used since it allows water to sit on top which is the 
worst case scenario. The antenna on the towfish was connected to the receiver on the boat 
via 60 feet of RG-213 coaxial cable where it was combined with the differential correction 
information being broadcast by MBARI. The eight channels of information that were being 
sampled by the Tattletale unit were transmitted via modem over 100 feet of RG-58 cable 
using the XMODEM protocol. The DGPS information and the information received via the 
modem were supplied to a 386 laptop computer through two serial ports. The computer was 


used to provide a real time navigation solution by integrating the information received from 
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the serial ports with Kalman filtering techniques. The 386 also logged raw data for post- 
processing. 

The experiment started by ensuring that the towfish and the computer were able to 
Communicate and that the DGPS information was available. The towfish was lowered to 
the water and allowed to gather enough GPS information to initialize its program. The GPS 
receiver was tracking six satellites at the commencement of the experiment. Over the next 
one and one half hours seven different test runs were obtained which amounted to over 30 
minutes of raw data. The first four test runs amounted to primarily INS data due to minor 
control problems with the towfish. The last three test runs were representative of the 
mission profile of an AUV with dives to four to five feet in depth for approximately 30 
seconds followed by 10 to 15 seconds on the surface while obtaining DGPS information. 
There was no way to determine the precise time required for the GPS receiver to acquire 
enough satellites for a fix. However, by watching the screen output on the computer, it was 
qualitatively estimated that the receiver was typically able to obtain a DGPS fix within five 
seconds of broaching the water’s surface. This result is generally in agreement with that of 
the more quantitative evaluation of this effect reported in [Norton 94]. 

Typical results of at-sea testing are presented in Figure 22 through Figure 24. As 
discussed previously the towfish was allowed to obtain multiple DGPS fixes prior to the 
first dive in order to initialize the system. Once the initialization was complete the towfish 
was commanded to dive for approximately 30 seconds before surfacing for 10 to 15 
seconds. 

Figure 22 shows results of the sea test between the first two DGPS fixes. The towfish 
was surfaced at point 1. The towfish was then commanded to dive and allowed to navigate 
via the INS for approximately 30 seconds to point 2 where it was then commanded to 
surface. Point 3 depicts the towfish on the surface where it was allowed to update its 
position by obtaining a DGPS fix. Qualitatively, the data shows a good correlation to the 
actual track of the towing boat with the exception that sensed water speed appears to be 


overestimated. A quantitative analysis of the data shows that from the last DGPS fix at 
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Figure 21: At-sea testing with the towfish 


point | to the first DGPS fix at point 3 was approximately 70 m, while the distance that the 
INS determined was on the order of 280 m. The difference between the actual distance 
traveled and the estimated distance is off by a factor of 4. 

Figure 23 shows results of the sea test between the second two DGPS fixes. 
Qualitatively the INS track continues to have a good correlation to the track of the towing 
vessel. A quantitative analysis shows that the difference between the two DGPS fixes 
(points 3 and 5) is again approximately 70 m, while the distance that the INS determined 
has decreased from 280 m to approximately 225 m. This decrease is an indication that the 
Kalman filter may be attempting to correct for the position error through estimation of 
ocean current. The difference between the actual distance traveled and the estimated 


distance has been reduced to a factor of 3. 
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Figure 24 presents the data for the entire test run. A similar analysis of the data can be 
performed between each of the DGPS fixes. Between DGPS fixes 5 and 7 the distance 
traveled is 44 m, while the INS determined that it traveled 130 m, which is approximately 
a factor of 3. Analysis of the data appears to show that the Kalman filter was able to 
estimate the heading fairly accurately. However, there appears to be a significant error in 
the waterspeed sensor. There is a potential for error in the sensor since it was not possible 
to calibrate the device prior to testing. Further analysis shows that the Kalman filter was 
attempting to correct for this error in speed as is evidenced by the decrease in each 
subsequent correction between GPS fixes. The filter then appears to overcompensate 
between points 7 and 8 and 9 and 10. This overcompensation is hypothesized to be due to 
the Kalman filter attempting to estimate the ocean current. Confirmation of this hypothesis 


is an important area for future work. 
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E. SUMMARY 


This chapter provides an explanation of the experimental tests that were developed and 
performed to determine the feasibility of using an integrated GPS/INS system to navigate 
an AUV. It also explains and analyzes the results of the static tests, cart tests, and at-sea 
testing. 

Static testing was performed to determine the suitability of the Systron-Donner IMU 
and differential GPS for testing purposes. Since the 10 Hz antialiasing filter discussed in 
[Schubert 95] was untried, it was important to test the output of the IMU after passing 
through this filter. The filter appears to work well and produces the desired results. Since 
DGPS was to be used as truth data, it was important to obtain a quantitative assessment of 
the accuracy of the system. The root sum square of the error show that the GPS system used 


is capable of producing navigation fixes well within the 100 m requirements of SPS. Also, 
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Figure 24: Typical results of at-sea tests 


the DGPS system is capable of producing results in the one to two meter range which is 
well with the 10 m required by the SANS mission profile. 

Cart testing was performed prior to at sea testing in order to establish the system was 
fully functional in its operational configuration. The tests were performed by placing the 
towfish on a wheeled cart and pushing it around a surveyed course. The results showed that 
the GPS/INS system was functioning properly. It also showed a good correlation to the 


surveyed track and provided a way to obtain initial values for the Kalman filter gains. 


62 








The mission of the Phoenix requires that the SANS and the antenna mounted on the 
AUV be as undetectable as possible. This requires that the antenna protrude as little as 
possible above the water’s surface. This makes the antenna susceptible to wave action and 
possible loss of GPS signal. The at-sea testing showed that an antenna that is mounted on 
an AUV can expect to track sufficient satellites to provide GPS fixes within a relatively 
short amount of time after surfacing. The concept of using a relatively inexpensive IMU 
with limited accuracy coupled with a GPS was proven to be a viable solution to the 
challenge of shallow water AUV navigation. However, further at sea testing and gain 


tuning will perfect this solution and produce better estimates of system accuracy. 
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Vil. SIMULATION 


A. INTRODUCTION 


The SANS software utilized for this research was prototyped in simulation prior to 
actual implementation. The goals of this simulation were to simulate navigation using an 
integrated INS and GPS, interface with high level software responsible for controlling an 
entire AUV mission, model the hardware devices of the AUV in software, and finally, to 
test and allow experimentation with Kalman filter designs. Attainment of these goals 
allowed a simulated AUV to find the shortest path around numerous obstacles while 
navigating through a series of way-points. Many of the algorithms and procedures utilized 
were directly translated and implemented into the actual SANS software design. This 
simulation helped to reduce the development time required for the SANS software as a 
whole. One of the primary reasons for this improvement was the 2D graphical 
representations of the motion of the AUV which the simulation produced. 

Due to the rapid prototyping utility of CLOS (Common LISP Object System) 
[Koschmann 90], the CLOS language was chosen for implementation of the simulation. 
The object-oriented nature of CLOS allowed the AUV and SANS to be represented as 
objects and classes. Each of these was tested individually using the CLOS interpreter. The 
interpretive nature of the language also allowed for quick testing of each individual method 
in a class and inspection of the slot values of individual objects to determine their status. 

This chapter will provide an overview of the simulation architecture and where it fits 
into the Rational Behavior Model (RBM) as described in [Byrnes 93]. It also highlights 
some of the important aspects of the implementation and some of the differences between 
the simulation and the actual SANS code. Finally a discussion of the results of the 


simulation and possibilities for further use is presented. 
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B. CODE ARCHITECTURE 


The Rational Behavior Model software architecture (RBM) [Byres 93] provides a 
method of dealing with the diversity of the processes involved in controlling an 
autonomous vehicle. This diversity is due to the inherent differences between high-level 
processes based on artificial intelligence (AI) techniques, low-level processes involving 
engineering and robotics concerns, and coordination processes which must link the two. To 
deal with these varying requirements, the RBM divides the processes into three levels. 
These levels are termed the Strategic level, which handles mission control, the Tactical 
level, which determines what actions are necessary to satisfy the demands of the Strategic 
level, and the Execution level which carries out those actions in real time. Figure 25 
illustrates where the code implemented for the simulation of this thesis fits into RBM. The 
objects contained in the dashed circle represent the objects which were implemented in 
prototyping the SANS. Those objects outside the dashed circle were used to drive the 
navigation software. The SANS objects lie entirely within the Tactical and Execution 
levels. 

The Navigator, INS and replanner classes are part of the tactical level. These objects 
represent software in the actual SANS and form the basis for the design of the real-time 
software used in this research. Their basic role in the simulation is to respond to navigation 
related commands given by the OOD object by processing information acquired from 
hardware objects at the execution level. 

Those objects which appear as part of the execution level in Figure 25 are considered 
hardware. These include the water-speed, IMU, GPS, UHF, Auto-Pilot and the Sub itself. 
The hardware object classes, with the exception of the AUV class, are derived from the 
black box class. The black box class itself is derived from a base class called rigid body. 
The sub class is descended directly from the rigid body class and it instantiates an object 
from each of the other hardware classes when instantiated itself. Figure 26 illustrates these 


inheritance relationships. 
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Figure 25: SANS objects in the RBM hierarchy 


There are several objects which support the simulation that are related only in an 
indirect manner to the navigation problems. In order to approximate the mechanics of real- 
world motion, the SANS simulation includes an large amount of kinematics code. The 
methods of the euler-angle-rigid-body class control the simulated motions of those objects 


derived from it using numerous matrix manipulation functions. The strobe-camera class 
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supports the graphical depiction of the motions of the simulated AUV. Figure 27 shows the 


image of the AUV created by this class. 
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Figure 26: SANS simulation class hierarchy 





Figure 27: Wire frame depictions of AUV 
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C. IMPLEMENTATION 


The simulated geometric world used in implementing the SANS simulation was kept 
as simple as possible. All positions are stated in coordinates relative to the origin. Courses 
and headings are expressed in radians measured clockwise from the “north” or positive x- 
axis. Obstacles are represented as ordered sets of vertices which specify normal or counter 
clockwise polygons as described in [Kanayama 95]. No attempt was made to model the 
complex hydrodynamic forces present in the real-world AUV environment. Figure 28 
depicts the “world” in which the simulated AUV navigates. The following sections 


describe the functionality corresponding to each software class. 


1. Navigator Class 


The navigator acts as the primary controller for all navigation tasks. Once told to 
proceed to a specific fix, it will continue to direct the auto-pilot towards the fix position 
until the way-point representing it is captured or an abort command is received from the 
OOD object. Throughout any simulated mission, the navigator will provide information as 
requested by higher level objects for use in tactical and strategic decision making. The basic 
tasks of the Navigator object are to determine current position based on inputs from the INS 
and GPS object inputs, manage and update navigational way-points and intermediate sub- 
way-points, and determine the course to the next way point. 

The state of the navigator at any given time is based on the contents of its five 
slots. These contain the target way-point to which it is currently proceeding, the sub-way 
points it must traverse while transiting to the target way-point, and an ordered list of way- 
points which must be captured in order to complete the mission. These slots are continually 
updated through out a mission. 

Update of the status of the navigator is driven by orders and queries sent to the 
navigator by the OOD object. Without regard to content, whenever an order or query is 


received, the navigator commands the AUV to update its position by a set time slice and 
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Figure 28: Geometric world 


requests from the INS object its latest estimate of the current position. Only after these tasks 
have been completed does the navigator inspect the order or query and attempt to satisfy it. 

Orders and queries are received as text strings via similar methods termed order and 
query respectively. Case statements are used in these two methods to determine what action 
is necessary. Orders, unlike queries, are designed to initiate action and require no response 
other than acknowledgment that the message was received. Queries on the other hand 
constitute a request for information. On receipt of either an order or a query, the navigator 
calls a sequence of methods as it performs the actions necessary to satisfy it. In this 
simulation, responses to some communications from the OOD object were left as stubs for 
future work. 

The Navigator accomplishes way-point control through four methods. The simplest 


method determines if the current target way-point has been captured by examining the 
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distance between the estimated position of the AUV and the target-way-point. The other 
methods accomplish their purpose by destructive updates to the way-point list, sub-way- 
point list, and target-way-point slots. Update of the sub-way-point list is accomplished via 
the replanner object each time a new target-way-point is designated. This allows the AUV 
to follow the shortest path around simulated obstacles to the new target-way-point. Updates 
of the target-way-point and way-point list slots take place each time the Navigator is told 
to proceed to a new way-point. Overall updates are based upon the orders of the OOD 
which determines to what location (and hence what way-point) the mission will next 


proceed. 


2. Replanner Class 


The replanner solves the shortest path planning problem between any two way-points 
in a polygon environment. The algorithms used to arrive at a solution are based upon the 
techniques described in [Kanayama 95]. The replanner takes as input arguments a starting 
point, an end point and several sets of vertices. The sets of vertices describe convex 
polygons which represent simulated obstacles. Upon completion of execution the method 
returns a subset of the vertices which describe the shortest path from the start point to the 
end point. For purposes of this shortest path planning problem, the AUV is considered to 
be a dimensionless vehicle or a point robot. 

The first step in solving the shortest path problem involves finding all “tangents” or 
possible path segments in the polygon world. These “tangents” can be divided into three 
types of line segments; tangents from the starting point to polygons, common tangents 
between polygons and tangents from polygons to the end points. Figure 29 shows the 
tangents in an example polygon world. 

The second step in solving the shortest path problem requires that a determination be 
made as to the visibility of each tangent. Any tangent which enters one of the obstacle 
polygons is said to be invisible. There are several such invisible tangents in Figure 29. 


Visibility determination must be made for each individual tangent. The exact method used 
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to determine the visibility of a tangent will be based upon the type of tangent. It will, 
however, involve checking for intersection between the “tangent” and the line segments 
which make up the polygon obstacles. The visibility test in the polygonal world is an 
essential task in shortest path planning, but is one of the most time-consuming ones. Figure 
30 shows the example polygon world with the invisible tangents removed. 

Following the visibility test, a weighted graph ia available to which a modified Dykstra 
search is applied. This graph is composed of the visible “tangents,” the line segments 
making up the boundaries of obstacle polygons, the vertices describing the obstacle 


polygons, and the original start and end points. Figure 30 is the graph on which search 


would be run. The shortest path generated by the search is specified using as an ordered list 


Goal 





Figure 29: All tangents in an example polygon world 


of polygon vertices. This list could be returned by the replanner to the navigator which 
could designate the vertices it contains as the sub-way-points. The sub-way-points would 


then be followed when travelling from one location (way-point) to another (Figure 31). 


cp: 











Figure 30: Visible tangents in an example polygon world 


Goal 





Start 


Figure 31: Shortest path determined from visibility graph 
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3. INS Process Class 


The INS acts as a test bed for the Kalman filter design described in chapter V. It performs 
simulated dead reckoning in-between GPS fixes to maintain an updated estimate of the 
current position. The state of the INS at any given time is based upon the contents of the 
estimated posture and velocity slots it contains. These values are continually updated based 
on inputs provided by the IMU, water speed and compass objects. These inputs include 
linear accelerations and angular rates in body coordinates which are translated, filtered and 


finally numerically integrated to obtain an estimate of the position and attitude of the AUV. 


4. Auto-pilot and Sub Classes 


In order to simplify the motion simulation model, the auto-pilot is assumed to be 
perfect”. The “perfect” auto-pilot assumption indicates the there is a tight coupling 
between the Sub and the auto-pilot. This assumption eliminates the necessity of modeling 
hydrodynamic forces. The auto-pilot receives speed, heading and depth commands from 
the navigator. It is then responsible for insuring that these parameters are meet by the rigid 
body Sub. With the exception of longitudinal velocity, which lags the requested input by a 
first order time lag, the velocity vector of the submarine is exactly what the auto-pilot has 
commanded. This implies that the angle of attack and sideslip at any given time are zero. 
The following computations are involved in determining the pitch rate, roll rate, yaw rate 
and longitudinal velocity of the sub. 

The rigid-body sub is assumed to be capable of instantaneously changing its pitch 
velocity vector. In other words there is no “mushing” as the vehicle establishes a new pitch 


attitude. This assumption allows use of the following first order equations for pitch rate(g ): 


0 = Ky (Zee iied ~ Zant) (7.1) 


commanded 


: ie K,(9 Deiat) (7.2) 


commanded _ 
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All turns are assumed to be flat with no angle of bank. Any roll rate (p )that might occur 


is damped according to the following equation. 
p= K, ; Pactual (7.3) 


The lack of side slip allows the use of a smooth motion steering function designed for 
line tracking [Kanayama 95] to be used to determine yaw rate, r. This steering function 
limits the AUV to only tangential motions. Thus the translational speed v and the rotational 
speed w or equivalently, the translational speed v and the path curvature c are the only 


degrees of freedom fully simulated. The speed of the AUV is assumed to be relatively low. 


For a vehicle which makes only tangential motions, the curvature changes 
continuously as a function of arc length, s. The curvature dc/ds at any given position 


along the vehicles track can therefore be expressed using the following steering function, 


dc . ; 
ds ~ a 3K, (Coeiuat = C yysived) 3K g (sina ‘= Paesirea) = Ke aa) Te) 


Ad is the signed distance between the vehicles actual position and the desired track. K, 
is related to the turning radius of the vehicle and determines the smoothness of the its 


dc 


motion. Using zr 


and the longitudinal velocity or translational speed, v, the yaw rate is 


determined as follows, 





At = a v At (7.5) 
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Parasitic and induced drags as well as other hydrodynamic forces are ignored. The first 
order time lag of longitudinal acceleration is computed as follows: 


eae (7.6) 


commanded U actual) 


5. IMU, Waterspeed and Compass Classes 

The IMU, Waterspeed and Compass objects provide the INS with the information 
necessary to dead reckon between fixes. The sensor reading of the waterspeed object is 
obtained from the x position of the sub velocity slot. Similarly the heading which the 
compass object provides is obtained by accessing the yw position in the posture slot of the 
sub. 

The IMU generates simulated accelerometer and angular rate sensor readings based on 


the movement of the sub. These values are derived from the velocity rates u, v, and w ,and 


the angular rates 6, 6 andy . The angular rates values are read in body coordinates directly 
from the velocity-growth-rate slot of the sub. The specific forces which would be sensed 
by areal world accelerometer are not directly available from the velocity growth rate slot. 


They are generated using the following (Newton-Euler) equations: 


x, = u— (wq+gsin6) (7.7) 
y, = u— (wp +ur) —gcosOsing (7.8) 
Zz, = W-— (ug + vp) — gcosOcosd (7.9) 


6. GPS and UHF Classes 

The GPS and UHF classes simulate the process of obtaining differential-corrected 
GPS information. Each time the GPS object is queried by the navigator for a new fix, a 
random function is invoked which simulates the variability in satellite reacquisition time 


by the receiver. Once three satellites have been acquired the method accesses the posture 
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slot of the sub to obtain the current x, y, z position. This position is then vector added to 
simulated pseudorange corrections provided by the UHF object to create a differential 


corrected position. This corrected position is returned to navigator. 


7. Clock Class 


The clock controls the timing and speed of execution of the simulation. Prior to each 
iteration or update of the simulation the clock is incremented by a set time interval. 
Through out the next iteration all objects will determine the current simulation time by 
calling the current-time method of the clock. Since all objects will obtain an identical time 
during any given iteration regardless of execution order, the processes appear to operate 


simultaneously. 


D. SIMULATION RESULTS 


Figure 32 shows the wire frame AUV executing a simulated mission while using way- 
point navigation. The ‘strobe’ effect is due to periodic snap shots taken by the strobe 
camera class. In this mission the AUV transited to and captured four way-points. The 
vehicle was initialized at the origin on a heading of North with a velocity of zero. Once the 
initialization was completed, the OOD commanded the navigator to proceed to the first 
way-point. The navigator then calculated the course to the way-point and commanded the 
auto-pilot to accelerate and turn to the calculated course. During transit, the OOD 
continually requested the navigator to determine if the way-point had been captured. Upon 
being informed of capture, the OOD commanded the navigator to proceed to the next way- 
point. As above, the navigator made the necessary course calculations and commanded the 
auto-pilot accordingly. This process was repeated for the remaining two way-points. 
Following capture of the fourth way-point, the navigator proceeded to a recovery point 
where the mission was completed. 

Throughout the simulated mission the INS calculated estimates of the AUV’s position. 
These estimates were found to contain unacceptably large position errors. At the time of 


the writing of this thesis, the source of these errors was undetermined. However, the 
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Figure 32: AUV way-point navigation 


simulation enhanced the software engineering of the real-time SANS software and provides 


a basis for future analysis of both real-time and simulated navigation problems. 


E. SUMMARY 





The primary goal of this simulation was to simulate AUV navigation using an 


integrated INS and GPS system. The greatest benefit to this research was the ability of the 
simulation to act as a test bed for various filter and navigator designs. The simulation goes 
beyond the basic navigation problems of an AUV, however, and attempts to examine where 


the navigation problem fits into the RBM hierarchy and controlling software as a whole 
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This secondary purpose accounts for the extensive interface capabilities of the navigator 
class. 

In order to take advantage of the object oriented paradigm, CLOS was chosen as the 
prototyping language. This made it possible to represent the many parts of the SANS as 
individual objects. The interpretive nature of CLOS sped testing and development. 

The primary differences between actual SANS and the prototype SANS used in this 
simulation are due to simplifying assumptions regarding the environment in which the 
simulated AUV operates and the motions of the simulated AUV. Unlike the actual SANS, 
the simulation SANS operates in a nearly perfect environment. All errors in the current 
configuration are due to the numerical inaccuracies of the machine on which the simulation 
is run. Additional differences lie in the simplified communications between the simulated 
software and hardware objects. In the actual SANS implementation, these problems 
required a great deal of labor to overcome. 

Future uses of the simulation might take on many forms. With the addition of 
simulated noise, improved Kalman filters might be tested and optimized. Simulated 
missions might be run to further test the RBM hierarchy. Auto-pilot designs could be tested 
by removing the “perfect” auto-pilot assumption and adding realistic hydrodynamic forces, 
either directly to the rigid-body sub class or through networked interaction with the NPS 
AUV Virtual World [Brutzman 94]. 
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VIII. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 


The purpose of this thesis was to develop and test an integrated navigation unit using 
INS and GPS. The standards against which this unit can be evaluated are the requirements 
for a Small Autonomous Under Water Vehicle (AUV) Navigation System (SANS) 
described by[Kwak 93]. Evaluation of the concept involved two major phases. The first 
utilized computer simulation to determine where the unit might fit into a larger software 
hierarchy and test various concepts and methodologies. The second phase progressed into 
the actual implementation and testing of the GPS/INS integrated unit. This testing began 
with land-based cart tests and eventually advanced to open-water experiments where the 
unit was placed in a towed body behind a boat in both submerged and surfaced states. 
Examination of the GPS/INS unit in the light of the research questions which guided this 
experiment demonstrates the ability of the unit to meet SANS requirements. 

The navigation system to be installed in the NPS Phoenix would be simpler than the 
one developed and tested in this research. This is due to the nature of the tow experiments 
performed in this research. Actual installation would eliminate the need for the hardware 
and software required to allow communication between those components of the system 
placed in the boat during the tow tests and those located in the towed body. In the final 
combined configuration, when all components are located within the Phoenix itself, there 
will be no need for the modems, tattle-tale or software implementing the XMODEM 
protocol. The computer simulation of the navigation system indicates that this simplified 
version will fit well with the hardware and software architectures of the Phoenix itself. 

The simulation developed in conjunction with this research demonstrates the 
feasibility of an AUV navigating from point to point while conducting open ocean-transit, 
given the ability to accurately estimate the vehicle position. The software desion 
establishes a system of way-points and then calculates the courses and headings necessary 


to transit from one to another. Water and land based tests showed that a integrated 
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navigation system combining GPS and INS is capable of providing a sufficiently accurate 
position to support this way-point navigation system. 

The simulation prototypes a software interface capable of communicating with AUV 
controlling programs based on the Rational Behavior Model (RBM) [Byrnes 93]. The 
prototyped interface is capable of responding to a large number of commands which can be 
used to control a variety of mission types. Future plans for Phoenix indicate that the 
controlling software architecture will be based upon the RBM [Healey 95]. 

The open-water experiments conducted in conjunction with this research tested the 
capability of an integrated GPS/INS navigation system to obtain accurate differential GPS 
fixes. Throughout these tow tests the system was able to quickly reacquire at least three 
satellites and provide accurate fixes using differential corrections. Incorporation of the 
system into the Phoenix will provide it with this same capability. 

Review of the simulation and examination of the experimental data indicate that the 
GPS/INS integrated unit developed in this research is capable of meeting the SANS 
requirements. All the requirements for a SANS were either meet or demonstrated to be 
obtainable with further research. Optimization of the INS filter gains and calibration of the 
various sensors making up the unit should allow it to meet the accuracy requirements in all 


regimes. 


B. RECOMMENDATIONS FOR FUTURE WORK 


There are many ways to build on the foundation this research has established. Much 
work remains to be done to perfect the Kalman filter design incorporated into the INS. This 
work can proceed along the same pattern established here by further modeling using and 
computer simulation followed by implementation into the real-time software and more 
rigorous tow tests. The following paragraphs will identify some specific areas and 
directions in which to move ahead. 

Several simplifying assumptions were made during the construction of the computer 


simulation. These simplifications can be divided into two major areas. The first of these is 
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the absence of a realistic error model. The addition of computer generated noise to the 
simulated sensor readings will create an environment in which the Inertial Navigation 
System (INS) can be more fully tested and optimized. Removal of the “perfect” auto-pilot 
assumption from the simulation will be another improvement. The application of a realistic 
model of the various hydrodynamic forces which are present during real-world AUV 
operations will produce more true-to-life accelerometer and rate readings for processing by 
the navigation system. The virtual world described in [Brutzman 94] presents an 
opportunity for further experimentation in this area. Further study can also be applied to the 
replanner class to make it capable of not only providing the shortest path but also a safe 
path in the correct coordinate system. 

Much calibration and bench testing of the SANS remains to be done. The water-speed 
sensor in particular is still largely untested. The 10-hertz filter is effective, but nevertheless 
runs at excessive operating temperatures. This indicates greater-than-necessary power 
consumption and the possibility of premature failure. The drift rate of the Motion Pack unit 
itself is unknown. Bench testing of this IMU will provide more insight into the bounds of 
its accuracy. 

The final step in this research will be incorporation of the SANS into the NPS AUV. 
The first step in this evolution will include simplification of the hardware/software 
interface. The Navigator object will require extensive modifications to make it more 
closely resemble the navigator object implemented in the simulation. Additional 
capabilities will include the ability to manage and calculate headings towards geographical 


fixes and response capabilities for to a variety of orders and queries. 
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APPENDIX A: Real Time Navigation Source Code (C**) 


A. TOWTYPES.H 


#ifndef TOWTYPES_H 
#define_ TOWTYPES_H 


#include <stdio.h> 
#include <dos.h> 
#include <time.h> 


#include " globals.h" // Types used by serial communications software 


#define GPSBLOCKSIZE 68 // Size of Motorola @@Ba position message 
#define PACKETSIZE 133// Size of packet received via X-modem protocol 


#define ONE_G 32.2185// One g in feet per second 
#define GRAVITY -32.2185 // In feet per second 


#define TicksToSecs(x) ((double) ((10 * x) / 182)) 


typedef char ONEBYTE; 
typedef short TWOBYTE; 
typedef long FOURBYTE; 


typedef unsigned char UNSIGNED_ONEBY TE; 
typedef unsigned short UNSIGNED_TWOBYTE; 
typedef unsigned long UNSIGNED_FOURBYTE; 


// Holds lat/long expressed in milseconds 
struct latLongMilSec { 

long latitude; 

long longitude; 


Is 
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// Holds a latitude or longitude expressed in hours minutes and degrees 
struct T.GEODETIC { 

TWOBYTE degrees; 

UNSIGNED_TWOBYTE minutes; 
double seconds; 


}; 


// Holds a latitude and longitude expressed as T.CGEODETICs 
struct latLongPosition { 
T GEODETIC latitude; 
T_GEODETIC longitude; 
F 


// Holds a grid position 
struct grid { 
double x,y,z; 


js 


//3 X3 matrix 
struct matrix { 
float element[3][3]; 
}; 


//3 X 1 matrix or vector 
struct vector { 
float element[3]; 


}s 


// Oversize area to hold a GPS message 
typedef BYTE GPSdata[2 * GPSBLOCKSIZE]; 


// Defines a type for storing INS packets 
typedef BYTE PACKET[PACKETSIZE]; 
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// Structure for passing around various types of INS information. 
// The positions in the sample field of a stampedSample structure 

// sample[{Q]: x acceleration 

// sample[1]: y acceleration 

// sample[2]: z acceleration 

// sample[3]: phi 

// sample[4]: theta 

// sample{5]: psi 

// sample[6]: water speed 

// sample[7]: heading 


struct stampedSample { 
grid est; //position as estimated by the INS. 
double sample[8]; //sampler converted sample. 
PACKET samplePacket;  //raw packet of samples. 
struct time timeStamp; _//time the sample arrived. 
float deltaT; 

ie 


#endif 


B. LOCATION.H 


//Conversion constants for location of 36:35:42.2N and 121:52:28.7W 
#define LatToFt 0.10134 //converts degrees Latitude to ft 

#define LongToFt 0.08156 //converts degrees Longitude to ft 

#define HemisphereConversion -1 //-1 if west of of Greenwich 


//lf variation is west then magvar should be negative 
#define RADIANMAGVAR 0.261799 // Local area Magnetic variation in radians 


C. TOWFISH.CPP 


#include <stdlib.h> 
#include <stdio.h> 
#include <string.h> 
#include <iostream.h> 


#include "towtypes.h" 
#include “nav.h" 


int breakHandler(void); 
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void printPosition (const latLongPosition&); 


[RERERREREERRKRERE AERA BER EE EEE EERE EER EAA AEA EH HARA EEE BBR EEA 


PROGRAM: Main 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Drives the navigator and its associated software. Counts 
the positions displays each to the screen. Exited only when control 
break is entered at the keyboard. 
RETURNS: 0 
CALLED BY: none 
CALLS: __ initializeNavigator (nav.h) 
navPosit (nav.h) 
printPosition 
breakHandler 


2 2 ee 6 Ag ee oe a Ae 2 2 2 2 2 26 a AH oe He 2K oe ee 2 9 ee AR 2 ee 2K He He 2 2 2 He A HK He HK A Ae A A A AK A ae HK AE / 


int 
main (int argc, char *argv[{]) 


ctrlbrk(breakHandler); // trap all breaks to release com ports 
setcbrk(1); // turn break checking on at all times 


char *dataFile, *scriptFile; 


switch (argc) { 
case 2: 
scriptFile = new char[strlen(argv[1])]; 
strcpy(scriptFile, argv[1]);//explicit script file only 
dataFile = "data";//default raw data file 
break; 
case 3: | 
scriptFile = new char[strlen(argv(1])]; 
strcpy(scriptFile, argv[1]);//explicit script file only 
dataFile = new char[strlen(argv[2])]; 
strcpy(dataFile, argv[2]);//explicit script file only 
break; 
default: 
scriptFile = "script";//default script file 
dataFile = "data";//default raw data file 
} 


cout << '\nRecording script in " << scriptFile; 
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cout << '\nRecording data in " << dataFile << endl; 


//Instantiate the navigator 
navigator nav1(scriptFile, dataFile); 


latLongPosition currentLocation; // Lat/Long of most recent fix 
Boolean fixReceived = FALSE;//True if a new fix was recieved 
int fixCount=0; // Count of navigation fixes recieved 


/AInitialize the navigator 
currentLocation = nav 1.initializeNavigatorQ); 


cout << 'NnInitialization Complete!\n"; 
cout << "Initial Position:\n"; 


//Print the initial position 
printPosition(currentLocation); 


while (TRUE) { 


// Attempt to get a fix from the navigator 
fixReceived = nav1.navPosit(currentLocation); 


if (fixReceived) { 
// New fix recieved 
cout << 'NnFix " << ++fixCount << endl; 
printPosition(currentLocation); 


} 
} 


[FECES REAR EE EE IO EA A A Ae Bee ee ore es oe emai cee Snes RRO Ie ones eA 


PROGRAM: printPositon 

AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 

FUNCTION:Displays position to the screen 
RETURNS: void 

CALLED BY:mail 

CALLS: none 


He 2 He 2 he ee 2 ee 2 he Ae 2 eA He eH He he 2H ae eH He ee A A He 2 eH He He He Ae Ae i 2 A He Ae 2 Ae 2 ae eB a 2 a AE He Hk 2 A 2 / 


void 
printPosition (const latLongPosition& posit) 
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{ 
cout << "Latitude: " << posit.latitude.degrees << ':'<< 
posit.latitude.minutes << ':' << posit.latitude.seconds << end]; 
cout << "Longitude: " << posit.longitude.degrees << ':'<< 
posit.longitude.minutes << ':' << posit.longitude.seconds << end]; 


} 


[RHR ERR RARE RK AR RAKE RR RRR AAA KK ARH E KKB BRA A HK RBA IE I EB IE I I HR 


PROGRAM: breakHandler 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Cleans up com ports upon program exit. 
RETURNS: 0 

CALLED BY: main 

CALLS: cleanup (portBank.h) 


oh oe 2k 2K 2 2k 2 he he 2 2 fe 2 fe fee 2 ee ee he 2 ee 2 he 2 he 2 2 2 2 2s 2 eof 2 he he 2 he 2 2 he 2 ee 2 2 a A 2s ae 2 eH A He EE / 


int 
breakHandler(void) 


COMports.cleanup(); 

exit(0); 

return0; // keep the compiler happy 
} 


D. NAV.H 


#ifndef _NAVIGATOR_H 
#define _ NAVIGATOR_H 


#include <fstream.h> 
#include <1ostream.h> 
#include <math.h> 


#include "towtypes.h" 
#include "gps.h" 
#include "ins.h" 
#include "location.h" 


// Converts milseconds to degrees 
#define MSECS_TO_DEGREES (1.0/(1000.0 * 3600.0)) 
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[RRR RRR HR AR HRA HA AR AK BH A HR HR A HB A CH RR RR I HE 


CLASS :navigator 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION:Combines GPS and INS information to return the current 


estimated position. 
Hie 9 3 ee fe 2k 2 2 2 2 oe he fe 2A 2 2 9 he eH 2A 2K 9 2 2 2 He ae A 2 2 HC A OK 2 He He he Ae A 2 He eH A A A A 2 2 He He A A 2 2 ee KK AB HK HE HE IK AK KE 


class navigator { 
public: 
//Constructor, opens script and data files 
navigator(char* scriptFile = "navScript", char* dataFile = "navData") 


: positionData (scriptFile), rawData(dataFile),celapsedTime(0.0){ } — 


//Destructor, closes script and data files 
~navigator() {positionData.close(); rawData.closeQ); } 


/fprovides the navigator's best estimate of current position 
Boolean navPosit (latLongPosition&); 


//initialize the navigator 
latLongPosition initializeNavigator(); 


private: 


INS ins1;//INS object instance. 
GPS gps1;//GPS object instance. 


ofstream positionData; // Position script file 
ofstream rawData;// Post processing output file. 


latLongMilSec origin; //lat-long of navigational origin 


//Write position information to script file 
void writeScriptPosit(int, latLongMilSec&, char); 


//Write an INS packet and its timeStamp to the outPut file 
void writeInsData(const stampedSample& drPosition); 


//Write a GPS message to the outPut file. 
void writeGpsData(const GPSdata& satPosition); 
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#endif 


//Returns the position in Miliseconds 
latLongMilSec getMilSec(const GPSdata&); 


//Convert position in milSec to degress, minutes, seconds and milsec 
latLongPosition milSecToLatLong(const latLongMilSec&); 


//Convert xy (grid) position to lat long 
latLongMilSec gridToMilSec(const grid&); 


//Converts lat/long to xy position 
grid milSecToGnrid(const latLongMilSec&); 


//Parses and returns the time of a GPS message. 
double getGpsTime(const GPSdata& rawMessage); 


float elapsedTime; 
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E. NAV.CPP 


#include "nav.h" 
#include "signal.h" 
#define SIGFPE 8// Floating point exception 


[PRA ARIA HHH RR RAR RAH RR EAA AA KE BAA AHHH HB BR RH HE ER IE I I I RE HEE 


PROGRAM: navPosit 
AUTHOR:Eric Bachmann, Dave Gay 
DATE: 11 July 1995 
FUNCTION:Provides the navigator's best estimate of current position. 
Attempts to obtain GPS and INS position fixes from the gps 
and ins objects and copies the most accurate fix available 
into the input argument 'navPosition’. Writes the raw position 
fix data to the output file for post processing. Sets a retum 
flag to indicate whether a valid fix was obtained. 
RETURNS:TRUE, a valid position fix is in the variable 'navPosition’. 
FALSE, otherwise. 
CALLED BY:towfish.cpp (main) 
CALLS: _— gpsPosition (gps.h) 
correctPosition (ins.h) 
insPosition (ins.h) 
getMilSec (nav.h) 
gridToMilSec (nav.h) 
milSecToGnid (nav.h) 
milSecToLatLong (nav.h) 
writeScriptPosit (nav.h) 
2H 2 ee 2A 2 aE ee He fe 6 AE ee Ee A A OK A 2 9 AK HH HK A HK A He 2 a A 2 He A A KOR A EK A A HR A RE A KE KE / 
void fpeNavPosit(int sig) 
{if (sig == SIGFPE) cerr << “floating point error in navPosit\n"; } 


Boolean 

navigator::navPosit (latLongPosition& navPosition) 

{ 
signal (SIGFPE, fpeNavPosit); 
GPSdata satPosition;  // the latest GPS position 
stampedSample drPosition; // the latest INS position 
latLongMilSec gpsMilSec; // the latest GPS position in milseconds 
latLongMilSec insMilSec; // the latest INS position in milseconds 
static int fixCount(Q); 


//Get the INS and GPS positions 


Boolean insFlag = ins1.insPosition(drPosition); 
Boolean gpsFlag = gps1.gpsPosition(satPosition); 
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/[INS and GPS positions obtained? 
if GnsFlag && gpsFlag) { 


else { 


cout << '\nINS & GPS: ": 

// Write INS packet to out put file. 

elapsedTime += drPosition.deltaT; 
writeInsData(drPosition); 

/[Write GPS message to output file. 
writeGpsData(satPosition); 

//Parse position from GPS messsage 

gpsMilSec = getMilSec(satPosition); 

//Write milsec position to script file 
writeScriptPosit(++fixCount, gpsMilSec, 'G’); 

//Pass GPS position to INS object for navigation corrections. 
ins 1 .correctPosition(milSecToGrid(gpsMilSec), getGpsTime(satPosition)); 
//Covert position in mil sec to latitude and longitude. 
navPosition = milSecToLatLong(gpsMilSec); 

return TRUE; 


//Only INS position obtained? 
if (insFlag) { | 


cout << 'NnINS: "; 

// Write INS Packet to output file. 
elapsedTime += drPosition.deltaT; 
writeInsData(drPosition); 

insMilSec = gridToMulSec(drPosition.est); 
//Write milsec position to script file 
writeScriptPosit(++fixCount, insMilSec, 'T’); 
navPosition = milSecToLatLong(insMilSec); 
return TRUE; 


else { 

// Only GPS position obtained? 

if (gpsFlag) { 
cout << "\nGPS: "; 
// Write GPS message to output file. 
writeGpsData(satPosition); 
//Parse position from GPS messsage 
gpsMilSec = getMilSec(satPosition); 
//Write milsec position to script file 
writeScriptPosit(++fixCount, gpsMilSec, 'G’'); 
//Pass GPS position to INS object for navigation corrections. 
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insl.correctPosition(milSecToGrid(gpsMilSec) 
,getGpsTime(satPosition)); 
//Convert position in mil sec to lat/long. 
navPosition = milSecToLatLong(getMilSec(satPosition)); 


return TRUE; 
} 
else { 

return FALSE; // No new position available 
} 


} 


[RRR RAR RA AAA RBA RK AH RA A AH A EH AR A A A HH AR AH A A HH HC A A HK IR EE EE 


PROGRAM: writeScriptPosit 

AUTHOR: Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Writes the fix number, the position in milSec and the type 
of fix to the script file. 

RETURNS: _ void 

CALLED BY:navPosit (nav.cpp) 
initialPosit (nav.cpp) 

CALLS: None 


HO OR AK 2 2 A KK KK HH HH A EA AB AK AK A HB HH A He A A AK 2 He He He ae He a AK AK 2 He He A AK A I He a KK A A A I | 
void 
navigator::writeScriptPosit(nt fixNumber, latLongMilSec& posit, char fixType) 


{ 


} 


positionData << fixNumber << '' 
<< posit.latitude << '' 
<< posit.longitude <<'' 
<< fixType <<"' 
<< elapsedTime << endl; 
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PROGRAM:  writeInsData 

AUTHOR: Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Writes the packet and the time stamp contained in a stamped 
sample to the out put file for post processing. 

RETURNS: void 

CALLED BY:navPosit (nav.cpp) 

CALLS: None 


BR 6 2K 6 2K 26 2H ae 2 HR 2 A AE AK A I AR A RO I A HH A AR 2 HO He KR AK HK AK HK HE KK | 
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void 
navigator::writeInsData(const stampedSample& drPosition) 
{ 
for( int j= 0; | < PACKETSIZE; j++) { 
rawData << drPosition.samplePacket[j]; 
} | 
rawData << drPosition.timeStamp.ti_min 
<< drPosition.timeStamp.ti_hour 
<< drPosition.timeStamp.ti_hund 
<< drPosition.timeStamp.ti_sec << end]; 


} 


[78 RAE AR He A A 2 A ae A HR A A A HA AK HH HE A 2 AR A A ACR ee 2 2 ae 2K 2 ae 2 2 9 2 he Hk 2k ek 2 a 2k 3k kK A IK HK 


PROGRAM:  writeGpsData 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Writes araw GPS message to the out put file for post 
processing. 

RETURNS: void 

CALLED BY:navPosit (nav.cpp) 

CALLS: None 
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void 
navigator:: writeGpsData(const GPSdata& satPosition) 
{ 
for( int j = 0; } < GPSBLOCKSIZE; j++) { 
rawData << satPosition|[}j]; 
} 
} 
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PROGRAM: initializeNavigator 
AUTHOR:Eric Bachmann, Dave Gay 
DATE: 11 July 1995 
FUNCTION: Obtains an initial GPS fix for use as a navigational origin for 
grid positions used by the INS object. Saves the origin and passes 
it to the INS object in latLong form. 
RETURNS:TRUE 
CALLED BY: towfish (main) 
CALLS: — gpsPosition (gps.cpp) 
correctPosition (ins.cpp) 
getMilSec (nav.cpp) 
milSecToGrid (nav.cpp) 
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latLongPosition 
navigator: :initializeNavigator() 


{ 


GPSdata satPosition; //gps position message 


// Loop until an initial GPS fix 1s obtained. 
while(!gps1.gpsPosition(satPosition)) 

f° 1} 
//Save navigational origin for later grid position conversions. 
origin = getMilSec(satPosition); 
//Write the initial position to the script file 
writeScriptPosit(O, origin, 'G'); 
//Pass GPS position to INS object for navigation corrections. 
ins 1 insSetUp(getGpsTime(satPosition)); 
//Return the initial position to the caller. 
return milSecToLatLong(origin); 
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PROGRAM: getMilSec 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Extracts a position in mili seconds from a Motorola (@@Ba) 
position contained in the input argument 'rawMessage' and returns it. 
RETURNS:The latitude and longitude in milseconds. 
CALLED BY: navPosit (nav.cpp) 
initializeNavigator (nav.cpp) 
CALLS:none. 


oe 2 26 9 oe a oe A a A a A AK AK He AK A AH HH A HAC KR A A I AK A KK AR IK A HK HK HK A A I IK A KE KE / 


latLongMilSec 
navigator::getMilSec(const GPSdata& rawMessage) { 


FOURBYTE temps4byte; 
latLongMilSec position; 


temps4byte =rawMessage[15]; 

temps4byte = (temps4byte<<8) + rawMessage[16]; 
temps4byte = (temps4byte<<8) + rawMessage[17]; 
temps4byte = (temps4byte<<8) + rawMessage[18]; 


position.latitude = temps4byte; 

temps4byte =rawMessage[19]; 

temps4byte = (temps4byte<<8) + rawMessage[20]; 
temps4byte = (temps4byte<<8) + rawMessage[21]; 
temps4byte = (temps4byte<<8) + rawMessage[22]; 


position.longitude = temps4byte; 


return position; 


} 
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[FREESE EERE TEESE ERE ET ee ee eee Ree ht eo ee ee eee eee en ee 
PROGRAM:milSecToLatLong 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Converts a position expressed in totally in mili seconds to 
degress, minutes, seconds and mili seconds and returns the result. 
RETURNS :The position in degress, minutes, seconds and mili seconds. 
CALLED BY: navPosit (nav.cpp) 
CALLS:none 
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latLongPosition 
navigator::milSecToLatLong(const latLongMilSec& milSec) { 


latLongPosition position; 
double degrees, minutes; 


degrees = (double)milSec. latitude * MSECS_TO DEGREES; 
position. latitude.degrees = (TWOBYTE)degrees; 


if(degrees < 0) | 
degrees = fabs(degrees); 


minutes = (degrees - (TWOBYTE)degrees) * 60.0; 
position.latitude.minutes = (TWOB YTE)minutes; 
position. latitude.seconds = (minutes - (TWOBYTE)minutes) * 60.0; 


degrees = (double)milSec. longitude * MSECS TO DEGREES; 
position.longitude.degrees = (TWOBYTE)degrees; 


if(degrees < Q) 
degrees = fabs(degrees); 


minutes = (degrees - (TWOBYTE)degrees) * 60.0; 
position.longitude.minutes = (TWOBYTE)minutes; 
position.longitude.seconds = (minutes - (TWOBYTE)minutes) * 60.0; 


retum position; 
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PROGRAM: gridToMilSec 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Convert a grid position to a latitude and longitude in mil- 
seconds and returns the result. 
RETURNS: The latitude and longitude in milseconds. 
CALLED BY:navPosit (nav.cpp) 
CALLS:none 
2 2 2 2s oe 2s 2s 2 a 9 a A 2 A oe 2 2 2 2 2 KA A He A A He a A A AK HK HH RK A IK A 2K 2 HH A AK A A A AE RK KE AE HK KH 
void fpeGridToMilSec(int sig) 
{if (sig == SIGFPE) cerr << “floating point error in gridToMilSec\n’; } 


latLongMilSec 
navigator::gridToMilSec(const grid& posit) 


signal(SIGFPE, fpeGridToMilSec); 
latLongMilSec latLong; 
cout << '\Nnposit.x =" << posit.x << "\nposit.y =" << posit.y << endl; 


//converts grid in ft to latitude 
latLong. latitude = origin.latitude + (posit.x / LatToFt); 


//converts grid in ft to longitude 
latLong.longitude = origin.longitude + 
HemisphereConversion * (posit.y / LongToFt); 


return latLong; 


} 
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PROGRAM:milSecToGrid 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Convert a latitude and longitude scureaseat in milseconds to 
a grid position based on the lat/long of the grid origin. 
RETURNS :The grid position 
CALLED BY:navPosit (nav.cpp) 
initializeNavigator (nav.cpp) 
CALLS: none 
COMMENTS: | altitude is always assumed to be zero. 
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//Converts latitude/longitude to xy coords in ft from origin 
grid 

navigator::milSecToGrid(const latLongMilSec& posit) 

{ 


grid position; 


position.x = (posit.latitude - origin.latitude) * LatToFt; 
position.y = HemisphereConversion * 

(posit.longitude - origin.longitude) * LongToFt; 
position.z = 0; 


return position; 


} 


[EEE A BAR A He AE io A Rs He AE eh PR A AS AR RCS IS Ae Ae oR CES ae POTTS Ae ae Re Tote Ae A eee ASRS Recs ois senna ee 


PROGRAM: getGpsTime 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION:Parse the time of a gps message. 
RETURNS:The time of the gps message in seconds 
CALLED BY:navPosit (nav.cpp) 
initializeNavigator (nav.cpp) 
CALLS: none 
LEELA TEE eRe ee er ae ee ee ee ee Rane me ee eee eee a eee | 
double 
navigator::getGpsTime(const GPSdata& rawMessage) 
{ 
UNSIGNED_ONEBYTE _ tempchar, hours, minutes; 
UNSIGNED_FOURBYTE _ tempu4byte; 
double seconds; 


hours =rawMessage[8]; 
minutes = rawMessage[9]; 


tempchar 

tempu4byte 
tempu4byte 
tempu4byte 
tempu4byte 


= rawMessage[ 10]; 
= rawMessage[1 1]; 
= (tempu4byte<<8) + rawMessage[12]; 
= (tempu4byte<<8) + rawMessage[13]; 
= (tempu4byte<<8) + rawMessage[14]; 


seconds = (double)tempchar + (((double)tempu4byte)/1.0E+9); 


return hours * 3600.0 + minutes * 60.0 + seconds; 
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F. GPS.H 


#ifndef GPS _H 
#define GPS_H 


#include "portbank.h" 
#include "towtypes.h" 
#include "gpsbuff.h" 


[BARA AA AAR HAH RAHA RRA RAH HAAR A RAHA AHR A HAAR HAH RAH HRA EAA EE AE 


CLASS: gps 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION:Reads GPS messages from the GPS buffer. Checks for valid 


checksum and minimun number of satelites in view. 
2H 9 A aK a AA a a A A HA CA A I A A A KR AR A OK A HK OK AK He HI EB A A I RK RB I EK KE EK AE / 


class GPS { 
public: 
//Class Constructor 
GPSQ : port2(COMports.Init(COM2, BYTE(3), b9600, 
NOPARITY, BYTE(8), BYTE(1), NONE, messages)) { } 


//returns the latest gps position and a flag 
Boolean gpsPosition (GPSdata&); 


private: 


//buffer for gps data 
GPSbuffer messages; 


//instantiates serial port communications on comm2 
bufferedSerialPort& port2; 


//calculates the check sum of the message 
Boolean checkSumCheck(const GPSdata); 


I 


#endif 
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G. GPS.CPP 


#include <math.h> 
#include "gps.h" 
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NAME: gpsPosition 
AUTHOR: Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: | 
Determines if an updated gps position message is available and 
copies it into the input argument 'rawMessage’. If the message 
has a valid checksum and was obtained with atleast three 
satelites in view, a ‘TRUE is returned to the caller, 
indicating that the message is valid. 
RETURNS: TRUE, if a valid position message is contained in the 
input argument. 
CALLED BY: navPosit (navigator.h) 
CALLS:Get (buffer.h) 
checkSumCheck (gps.h) 


Inbindunnnnbinbuikicbiobiiopbvkinenibvnnkidicodeiidinoikiscioiookikiiokbisieisiokskeisok 


Boolean 
GPS::gpsPosition (GPSdata& rawMessage) 


{ 
if (messages.Get(rawMessage)) { 


// Check for a valid check sum and more the 3 satelites 
return Boolean((checkSumCheck(rawMessage)) && (rawMessage[39] >= 3)); 
} 


else { 
return FALSE; // No updated position is available. 
} 
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PROGRAM: checkSumCheck 

AUTHOR: Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: 

Takes an exclusive or of bytes 2 through 64 in a Motorola format (@@BA) 
position message and compares it to the checksum of the message of the 
message. 

RETURNS: TRUE, if the message contains a valid checksum 

CALLED BY: gpsPosition (gps) 

CALLS: none 


ake 9 2 A aK A a a KK KK HK KK HA a A a HE A A oH A a HK a A a A ae eH A EAR A HK I HK HK A AK HK KK IE / 


Boolean 
GPS::checkSumCheck(const GPSdata newMessage) 


{ 
ONEBYTE chkSum(0); 


for (int i = 2; i < 65; 1++) { 
chkSum “= newMessage[i]; 


} 


return Boolean(chkSum == newMessage[65]); 


} 
H. INS.H 


#ifndef INS _H 
#define INS _H 


#include <time.h> 
#include <math.h> 
#include <dos.h> 

#include <stdio.h> 


#include <fstream.h> 
#include <iostream.h> 


#include "towtypes.h" 


#include "sampler.h" 
#include "location.h" 
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CLASS:ins 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Takes in linear accelerations, angular rates, speed and 
heading information and uses kalman filtering techniques to return 
a dead reconing position. 


2 6 6 2 ae 2 ea a A A He HG A eR AR He A HH eH AK HA HH HH AK He HK 2 eH He He He HE He A 2 He HK 
class INS { 
public: 


//Constructor initializes gains 
INSQ; 


~INSQ {attitudeData.close(); } 


//returns the ins estimated position 
Boolean insPosition(stampedSample&); 


/{Updates the x, y and z of the vehicle posture 
void correctPosition(const grid&, double); 


//records the initial position of and time 
void insSetUp(double); 


private: 
ofstream attitudeData;// Post processing output file. 
double posture[6]; _// ins estimated posture (x y z phi theta psi) 
double velocities[6]; // ins estimated linear and angular velocities 
// x-dot y-dot z-dot phi-dot theta-dot psi-dot 


double current[3]; _// ins estimated error current (x-dot y-dot z-dot) 


struct time lastTime; _//time of last ins position fix 
double lastGPStime; //time of last gps position fix 


sampler sam]; //sampler instance 
matrix rotationMatrix; //body to euler transformation matrix 


double biasCorrection[8]; //Software bias corrections for IMU rate sensors 
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// Kalman filter gains. 
float Konel, Kone2, Ktwo, Kthreel, Kthree2, Kfourl, Kfour2; 


// Finds the difference between two times of struct time type 
double findDeltaT (struct time& next, struct time& last); 


// Transforms from body coordinates to earth coordinates 
// and removes the gravity component 
void transformAccels (double[]); 


// Transforms water speed reading to x and y components 
void transformWaterSpeed (double, double[]); 


// Tranforms body euler rates to earth euler rates. 
void transformBodyRates (double[}); 


// Euler integrates the accelerations and updates the velocities 
void update Velocities (stampedSampled&); 


// Euler integrates the velocities and update the posture 
void updatePosture (stampedSampleé); 


// Builds the body to euler rate matrix 
matrix buildBodyRateMatrix(); 


// Builds the body to earth rotation matrix 
void buildRotationMatrixQ); 


// Convert magnetic direction based magnetic variation. 
double trueHeading(const double); 


//Calculates the imu bias correction during set up 
void calculateBiasCorrections(); 


//Applies bias corrections to a sample 
void applyBiasCorrections(double sample[]); 


// Post multiply a matrix times a vector and return result. 
vector operator* (matrix&, double[]); 


#endif 
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I. INS.CPP 


#include <iostream.h> 
#include "ins.h" 
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PROGRAM: ins (constructor) 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Constructor initializes kalman filter gains and linear and 
angular velocities. 

RETURNS: nothing 

CALLED BY: navigator class 

CALLS: none 


He ee EE HE EE 2 EEE HEE EE ER AH ER HR HE EE HE HK HR RAR RE A AE I RR CK / 


INS::INSQ : Konel(0.1), Kone2(0.1), 
Ktwo(0.6), 
Kthree1(0.5), Kthree2(0.5), 
Kfour1(1.0), Kfour2(1.0), 
attitudeData("attitude") 


velocities[O} = 0.0;// x dot 
velocities[1] = 0.0;// y dot 
velocities[2] = 0.0;// z dot 
velocities[3] = 0.0;// phi dot 
velocities[4] = 0.0;// theta dot 
velocities[5] = 0.0;// psi dot 


// Initialize error bias to zero 
current[0] = 0.0; 
current[ 1] = 0.0; 
current[2] = 0.0; 
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PROGRAM: findDeltaT 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Converts two times stored in the time structure type of 
dos.h into the time in seconds and returns the difference. 
RETURNS: difference in seconds between the two input times. 
CALLED BY: insPosit Gins.cpp) 

CALLS: none 
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double 
INS::findDeltaT (struct time& next, struct time& last) 
{ 


double present, past; 


present = next.ti_hour * 3600.0 + next.ti_min * 60.0 
+ next.ti_sec + next.ti_hund / 100.0; 
past = last.ti_hour * 3600.0 + last.ti_min * 60.0 
+ last.ti_sec + last.ti_hund / 100.0; 


// Did 2400 occur imbetween present and past? 
if (present < past) { 

present += 86400.0; 
} 


return present - past; 


} 
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PROGRAM: insPosit 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Make dead reckoning position estimation using kalman 
filtering. Inputs are linear accelerations, angular rates, speed and 
heading. Primary input data is obtained from a sampler object via the 
getSample method. This data is stored in the sample filed of a 
stampedSample structure called newSample. The sample field is then 
used as a working variable as the linear accelerations and angular 
rates it contains are converted to earth coordinates and integrated 
to determine current velocities and posture. The data is complimentary 
filtered against itself, speed and magnetic heading. 
RETURNS: position in grid coordinates as estimated by the INS 
CALLED BY: navPosit (nav.cpp) 
CALLS: getSample (sampler.cpp) 
findDeltaT (ins.cpp) 
transformBodyRates (ins.cpp) 
buildRotationMatrix (ins.cpp) 
transformAccels (ins) 


transformWaterSpeed (ins) 
2 2 2 2 2 2 A 2K ae ea 2K ae he He A A He A A He A He He He A eH HK KH HK HK I HK A A HH A AK He HA HK A A A A AK A AEE HK / 


Boolean 
INS::insPosition(stampedSample& newSample) 
{ 


double thetaA, phiA, xIncline, yIncline; 
double deltaT; // Integration interval 
double waterSpeedCorrection[3]; // Filter correction for drift 
// and water speed 
static float elapsedTime = 0.0; // Maintains elapsed time 
if (sam1.getSample(newSample)) { 
newSample.sample[7] = trueHeading(newSample.sample[7]); 
applyBiasCorrections(newSample.sample); 
cout << '\nx accel: " << newSample.sample[0] 


<<" y accel: " << newSample.sample[1] 
<<" z accel: " << newSample.sample[2] << endl; 
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cout << "\nphi dot: " << newSample.sample[3] 
<< " theta dot: " << newSample.sample[4] 
<<" psi dot: " << newSample.sample[5] << endl; 


cout << "water speed: " << newSample.sample[6] 
<< " heading: " << newSample.sample[7] << endl; 


deltaT = findDeltaT(newSample.timeStamp, lastTime); 


if (deltaT == 0) { 
cout << '\nZero divide error in insPosition\n"; 
printf("newSample.timeStamp: %2d:%02d:%02d.%02d\n", 
newSample.timeStamp.ti_hour, newSample.timeStamp.ti_min, 
newSample.timeStamp.ti_sec, newSample.timeStamp.ti_hund); 
printf(“lastTime: %2d:%02d:%02d.%02d\n", 
lastTime.ti_hour, lastTime.ti_min, 
lastTime.ti_sec, lastTime.ti_hund); 
deltaT = 0.2; 

} 


newSample.deltaT = deltaT; 


xIncline = newSample.sample[0] / GRAVITY; 
ylncline = newSample.sample[1] / (GRAVITY * cos(posture[4])); 


if (fabs(yIncline) > 1.0) { 
cerr << '\n Inclination error! \n": 
return FALSE; 
} 
//Calculate low freq pitch and roll 
thetaA = asin(xIncline); 
phiA = -asin(yIncline); 


//Transform body rates to transform euler rates. 
transformBodyRates(newSample.sample); 


//Calculate estimated pitch rate (phi-dot). 

velocities[3] = newSample.sample[3] + Konel * (phiA - posture[3]); 
//Calculate estimated roll rate (theta-dot). 

velocities[4] = newSample.sample[4] + Kone2 * (thetaA - posture[4}]); 
//Calculate estimated heading rate (psi-dot). 

velocities[5] = 


newSample.sample[5] + Ktwo * (newSample.sample[7] - posture[5]); 
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//integrate estimated pitch rate to obtain pitch angle 
posture[3] += deltaT * velocities[3]; 

//integrate estimated roll rate to obtain roll angle 
posture[4] += deltaT * velocities[4]; 

//integrate estimated yaw rate to obtain heading 
posture[5] += deltaT * velocities[5]; 


elapsedTime += deltaT; 


attitudeData << elapsedTime << '' << posture[3] << '' << posture[4] 
<< '' << posture[5| << endl; 


buildRotationMatrix(); 


//Transform accels to earth coordinates 
transformAccels(newSample.sample); 


//Transform water speed to earth coordinates 
transformWaterSpeed(newSample.sample[6], waterSpeedCorrection); 


//Calculate speed over the ground 
waterSpeedCorrection[0] += current[0]; 
waterSpeedCorrection[1] += current[ 1]; 


// Subtract out previous velocity and apply statistical gain 
waterSpeedCorrection[0] = 

Kthreel * (waterSpeedCorrection[0] - velocities[0]); 
waterSpeedCorrection[1] = 

Kthree2 * (waterSpeedCorrection[1] - velocities[1]); 


// Determine filtered accelerations 
newSample.sample{0] += waterSpeedCorrection[0]; 
newSample.sample[1] += waterSpeedCorrection[1]}; 


//Integrate accelerations to obtain velocities 

velocities[0] += newSample.sample[0] * deltaT; 
velocities[1] += newSample.sample[1] * deltaT; 
velocities[2] += newSample.sample[2] * deltaT; 


//Integrate velocities to obtain posture 
posture[0] += velocities[O] * deltaT; 
posture[1] += velocities[1] * deltaT; 
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posture[2] += velocities[2] * deltaT; 
lastTime = newSample.timeStamp; 


newSample.sample[0] = posture[0]; 
newSample.sample[1] = posture[ 1]; 
newSample.sample[2] = posture[2)]; 
newSample.sample[3] = posture[3]; 
newSample.sample[4] = posture[4]; 
newSample.sample[5] = posture[5]; 


newSample.est.x = posture[0]; 
newSample.est.y = posture[ 1]; 
newSample.est.z = posture[2]; 


return TRUE; 
} 
else { 
return FALSE;// New IMU information was unavailable. 
} 
} 
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PROGRAM: correctPosition 

AUTHOR: Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Reinitializes the INS based on a known position and compute 
apparent current based on past accumulated errors of the INS. It is 

called by the navigator each time a new GPS (true) fix is obtained. 
RETURNS:void 

CALLED BY: navPosit (nav) 

CALLS: none 
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void 
INS::correctPosition(const grid& truePosit, double positTime) 


double deltaT; 
// Correct for new day if necessary 
if (positTime < lastGPStime) { 


positTime += 86400; 
} 
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// Find time since last gps fix. 
deltaT = positTime - lastGPStime; 


// Detemine INS error since last gps fix 
double deltaX = truePosit.x - posture[0]; 
double deltaY = truePosit.y - posture[ 1]; 


// Reinitialize posture to known position (gps fix) 
posture[0] = truePosit.x; 

posture[1] = truePosit.y; 

posture[2] = 0.0; 


// Add gain filtered error to previous errors 
current[O] += Kfourl * (deltaX / deltaT); 
current[1] += Kfour2 * (deltaY / deltaT); 


// Save the time of the gps fix for next calculation 
lastGPStime = positTime; 


} 
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PROGRAM: insSetUp 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Initializes the INS system. Sets the posture to the origin. 

Initializes the heading using magnetic compass information. Initilizes 

the times of the last GPS fix and last IMU information. 

RETURNS:void 

CALLED BY:initializeNavigator (nav) 

CALLS: calulateBiasCorrections (ins) 
getSample (sampler) 
buildRotationMatrix (ins) 
transformWaterSpeed (ins) 
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void 
INS::insSetUp(double originTime) 
{ 


stampedSample newSample; 
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//Set posture to straight and level at the origin. 
posture[O] = 0.0; 
posture[1] = 0.0; 
posture[2} = 0.0; 
posture[3] = 0.0; 
posture[4] = 0.0; 


//set imu biases 
calculateBiasCorrections(); 


while(!sam1.getSample(newSample)) {/* */}; 


//set initial true heading 
posture[5] = trueHeading(newSample.sample[7]); 


//set initial speed 
buildRotationMatrix(); 
transformWaterSpeed(newSample.sample[6], velocities); 


// initialize times 
lastTime = newSample.timeStamp; 
lastGPStime = originTime; 


} 
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PROGRAM: transformAccels 

AUTHOR: Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Transforms linear accelerations from body coordinates to 
earth coordinates and removes the gravity component in the z direction. 
RETURNS: void 

CALLED BY: navPosit 

CALLS: none 
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void 
INS::transformAccels (double newSample[]}) 


vector earthAccels; 
newSample[0] -= GRAVITY * sin(posture[4]); 


newSample[1] += GRAVITY * sin(posture[3]) * cos(posture[4]); 
newSample[2] += GRAVITY * cos(posture[3]) * cos(posture[4]); 
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earthAccels = rotationMatrix * newSample; 


newSample[0] = earthAccels.element[0)]; 
newSample[1] = earthAccels.element[ 1]; 
newSample[2] = earthAccels.element[2]; 
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PROGRAM: transformWaterSpeed 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Transforms water speed into a vector in earth coordinates 
and returns them in the speedCorrection variable. 

RETURNS: void 

CALLED BY: navPosit 

CALLS: none 
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void 


INS::transformWaterSpeed (double waterSpeed, double speedCorrection[]) 


double water[3] = {waterSpeed, 0.0, 0.0}; 
vector waterVelocities = rotationMatrix * water; 


speedCorrection [0] = waterVelocities.element[0]; 
speedCorrection [1] = waterVelocities.element| 1]; 
speedCorrection [2] = waterVelocities.element[2]; 
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PROGRAM: transformBodyRates 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Tranforms body euler rates to earth euler rates 
RETURNS: none 

CALLED BY: insPosit 

CALLS: buildBodyRateMatrix 
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void 


INS::transformBodyRates (double newSample[]) 
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vector earthRates = buildBodyRateMatnix() * &(newSample[3]); 


newSample[3] = earthRates.element[0); 

newSample[4] = earthRates.element[ 1]; 

newSample[5] = earthRates.element/[2]; 
} 
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PROGRAM: buildBodyRateMatnx 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Builds body to Euler rate translation matrix. 
RETURNS: rate translation matrix 

CALLED BY: insPosit 

CALLS: none 
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matrix 
INS::buildBodyRateMatrix() 
{ 


matrix rateTrans; 


float tth = tan(posture[4}]), 
sphi = sin(posture[3]), 
cphi = cos(posture[3]), 
cth = cos(posture[4}]); 


rateTrans.element[0][0] = 1.0; 
rateTrans.element[0][1] = tth * sphi; 
rateTrans.element[0][2] = tth * cphi; 
rateTrans.element[1][0] = 0.0; 
rateTrans.element{1][1] = cphi; 
rateTrans.element[1][2] = -sphi; 
rateTrans.element[2][O] = 0.0; 
rateTrans.element[2]|[1] = sphi / cth; 
rateTrans.element[2][2] = cphi / cth; 


return rate Trans; 


} 
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PROGRAM: buildRotationMatrix 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION:Sets the body to earth coordinate rotation matrix. 
RETURNS: void 
CALLED BY: insPosit 
insSetUp 
CALLS: none 
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void 

INS::buildRotationMatrix() 

{ 

float spsi = sin(posture[5]), 

cpsi = cos(posture[5]), 
sth = sin(posture[4}), 
sphi = sin(posture[3]), 
cphi = cos(posture[3]), 
cth = cos(posture[4]); 


rotationMatrix.element[0][0] = cpsi * cth; 
rotationMatrix.element[0][1] = (cpsi * sth * sphi) - (spsi * cphi); 
rotationMatrix.element[0][2] = (cpsi * sth * cphi) + (spsi * sphi); 
rotationMatrix.element[ 1][0] = spsi * cth; 
rotationMatrix.element[1][1] = (cpsi * cphi) + (spsi * sth * sphi); 
rotationMatrix.element[1}[2] = (spsi * sth * cphi) - (cpsi * sphi); 
rotationMatrix.element[2][0] = -sth; 
rotationMatrix.element[2][1] = cth * sphi; 
rotationMatrix.element[2][2] = cth * cphi; 
} 
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PROGRAM:post multiplication operator * 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION:Post multiply a 3 X 3 matrix times a 3 X 1 vector and 
return the result. 

RETURNS: 3X 1 vector 

CALLED BY: 

CALLS: None 
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vector 
operator* (matrix& transform, double state[]) 


{ 


vector result; 

for (int i = 0; 1< 3; i++) { 
result.element[i] = 0.0; 
for (int j = 0; j < 3; j++) { 


result.element[i] += transform.element[i][j] * state[j]; 
} 
yo 
return result; 


} 
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PROGRAM: trueHeading 

AUTHOR: Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Convert magnetic direction to true based on local magnetic 
variation. 

RETURNS: true heading 

CALLED BY: insPosit 

insSetUp 

CALLS: none 
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double 
INS::trueHeading(const double magHeading) 


{ 
static double twoPi(2.0 * M_PI); 
double trueHeading = magHeading + RADIANMAGVAR; 


if (trueHeading > twoPi) { 


trueHeading -= twoPi; 


} 


return trueHeading; 
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PROGRAM: calculateBiasCorrections 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Calculates the initial imu bias by averaging a number of 
imu readings. 

RETURNS: none 

CALLED BY: insSetup 

CALLS:none 
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void 
INS::calculateBiasCorrections() 


{ 


int biasNumber(10); 
stampedSample biasSample; 


biasCorrection[3] = 0.0; 
biasCorrection[4] = 0.0; 
biasCorrection[5] = 0.0; 


for (int i = 0; 1 < biasNumber ; i++) { 
while(!sam1.getSample(biasSample)) {/* */}; 
biasCorrection[3] += biasSample.sample[3]; 
biasCorrection[4] += biasSample.sample[4]; 
biasCorrection[5] += biasSample.sample[5]; 
} 
biasCorrection[3] = -(biasCorrection[3 ]/biasNumber); 


biasCorrection[4] = -(biasCorrection[4]/biasNumber); 
biasCorrection[5] = -(biasCorrection[5]/biasNumber); 
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PROGRAM: applyBiasCorrections 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Applies updated bias corrections to a sample. 
RETURNS: void 
CALLED BY: insPosit 
CALLS: none 
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void 
INS::applyBiasCorrections(double sample[]) 
{ 
static const float biasWght(0.99), sampleW ght(0.01); 


biasCorrection[3] = (biasWght * biasCorrection[3]) 

- (sampleW ght * sample[3]); 
biasCorrection[4] = (biasWght * biasCorrection[4}) 

- (sampleW ght * sample[4]); 
biasCorrection[5] = (biasWght * biasCorrection[5]) 

- (sampleW ght * sample[5]); 


sample[3] += biasCorrection[3]; 
sample[4] += biasCorrection[4]; 
sample[5] += biasCorrection[5]; 


} 


J. SAMPLER.H 


#ifndef SAMPLER H 
#define SAMPLER H 


#include <time.h> 
#include <math.h> 
#include <dos.h> 
#include <conio.h> 
#include <stdio.h> 


#include "portbank.h" 


#include "towtypes.h" 
#include "packbuff.h" 
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#define xyAccelLimit ONE_G// Max accell in x and y diretion 
#define zAccelLimit 2 * ONE_G // Max accel in z direction 
#define rateLimit 0.872665// Max rotational rate in radians 
#define speedLimit 25.3 //Max water speed 

#define headingLimit 2 * M_PI 


#define rateUnits(angular) ((((angular-2048.0) / 2048.0 ) * 50.0) * (M_PI/180.0)) 
#define accelUnits(linear) (((linear-2048.0) / 2048.0 ) * GRAVITY) | 
#define zAccelUnits(linear) (((linear-2048.0) / 2048.0) * (2.0 * GRAVITY)) 
#define depthUnits(depth) (((depth - 819.0) / (4096.0-819.0)) * 180.0) 

#define waterSpeedUnits(speed) ((speed / 4096.0) * 25.3) //feet per second 
#define headingUnits(heading) (((heading - 81.92) / 4.096) * (M_PI/180.0)) 
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CLASS:sampler 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION:Formats, timestamps, low pass filters and limit checks IMU, 
water-speed and heading information. 

COMMENTS: This class is extremely dependent upon the specific 
hardware configuration. It is designed to isolate to the INS from 

these particulars. 
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class sampler { 
public: 
//Class Constructor 
sampler() : portl(COMports.Init(COM1, BYTE(4), b9600, 
NOPARITY, BYTE(8), BYTE(1), NONE, packets)), 
packets(portl) { } 


//checks for the arrival of a new sample and formats it. 
Boolean getSample(stampedSample&); 


private: 


//instantiates serial port communications on comm| 
bufferedSerialPort& port]; 


//ouffer for gps data 
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packetBuffer packets; 


//Averages the eight samples of a samplePacket 
void createSample(stampedSample& newSample); 


//Converts the voltages of a sample to real world units 
void formatSample (stampedSampledz); 


// Returns TRUE if all sample values are within limits 
Boolean inLimitSample(stampedSample& newSample); 


#endif 


K. SAMPLER.CPP 


#include <iostream.h> 
#include "sampler.h" 
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PROGRAM: getSample 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION:Prepares raw sample data for use by the INS object 

RETURNS: TRUE, if a valid sample was obtained 

CALLED BY:insPosit (ins) 
insSetup (ins) 

CALLS: Get (packetBuffer) 
createSample (sampler) 
formatSample (sampler) 
inLimitSample (sampler) 
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//checks for the arrival of a new sample 
Boolean 
sampler::getSample(stampedSample& newSample) 


{ 


// Attempt to obtain sample 
if (packets.Get(newSample.samplePacket)) { 
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//Time stamp the sample 
gettime(&newSample.timeStamp); 


//Low pass filter the sample 
createSample(newSample); 


//Convert Voltages to units 
formatSample(newSample); 


//Check for out of limit readings 
if (inLimitSample(newSample)) { 


return TRUE; 
po 
else { 
cerr << '\n Motion Pack Sample Out of Limits \n"; 
return FALSE; 
} 
} 
else { 


return FALSE; //Valid sample not available 
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PROGRAM: createSample 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Low pass filters eight closely spaced sets of sensor 
readings by summing the readings of each sensor and computing the 
average. 

RETURNS: void 

CALLED BY: getSample 

CALLS: none 
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void 
sampler::createSample(stampedSample& newSample) 


UNSIGNED_TWOBYTE tempNum; 


for (int i = 0; 1< 8; 1++) { 
newSample.sample[i] = 0.0; 
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} 


for i= 3;1< 131;) { 

tempNum = newSample.samplePacket[i++]; 

tempNum = (tempNum << 8) + newSample.samplePacket[i++]; 
newSample.sample[0] += double(tempNum/16); 

tempNum = newSample.samplePacket[i++]; 

tempNum = (tempNum << 8) + newSample.samplePacket[i++]; 
newSample.sample[1] += double(tempNum/16); 

tempNum = newSample.samplePacket[i++]; 

tempNum = (tempNum << 8) + newSample.samplePacket[i++]; 
newSample.sample[2] += double(tempNum/16); 

tempNum = newSample.samplePacket[1++]; 

tempNum = (tempNum << 8) + newSample.samplePacket[i++]; 
newSample.sample[3] += double(tempNum/16); 

tempNum = newSample.samplePacket[i++]; 

tempNum = (tempNum << 8) + newSample.samplePacket[i++]; 
newSample.sample[4] += double(tempNum/16); 

tempNum = newSample.samplePacket[i++]; 

tempNum = (tempNum << 8) + newSample.samplePacket[i++]; 
newSample.sample[5] += double(tempNum/16); 

tempNum = newSample.samplePacket[i++]; 

tempNum = (tempNum << 8) + newSample.samplePacket[i++]; 
newSample.sample[6] += double(tempNum/16); 

tempNum = newSample.samplePacket[i++]; 

tempNum = (tempNum << 8) + newSample.samplePacket[i++]; 
newSample.sample[7] += double(tempNum/16); 


newSample.sample[0] /= 8.0; 
newSample.sample{1] /= 8.0; 
newSample.sample[2] /= 8.0; 
newSample.sample[3] /= 8.0; 
newSample.sample[4] /= 8.0; 
newSample.sample[5] /= 8.0; 
newSample.sample[6] /= 8.0; 
newSample.sample[7] /= 8.0; 
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PROGRAM:formatSample 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Converts integers representing voltage readings into 
units which are useable by the INS. 

RETURNS: void 

CALLED BY:getSample 

CALLS: none 
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//Calls the methods to convert the voltages to real world units 

void 

sampler::formatSample (stampedSample& newSample) 

{ 
newSample.sample[0] = accelUnits(newSample.sample[0]); 
newSample.sample[1] = accelUnits(newSample.sample[1]); 
newSample.sample[2] = zAccelUnits(newSample.sample[2]); 


newSample.sample[3] = rateUnits(newSample.sample[3]); 
newSample.sample[4] = rateUnits(newSample.sample[4]); 
newSample.sample[5] = rateUnits(newSample.sample[5}); 


newSample.sample[6] = waterSpeedUnits(newSample.sample[6]); 
newSample.sample[7] = headingUnits(newSample.sample[7]); 


} 
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PROGRAM: inLimitSample 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION:Checks sensor readings to insure they are with in the 
limits of the sensor from which they originated. 

RETURNS: True, If all readings are with in limits. 

CALLED BY:getSample 

CALLS: none 
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Boolean 
sampler: :inLimitSample(stampedSample& newSample) 


Boolean limitFiag(TRUE); 
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if (fabs(newSample.sample[0]) > xyAccelLimit) { 
limitFlag = FALSE; 
cerr << '\n imu x accel out of limits\n"; 


} 

if (fabs(newSample.sample[1]) > xyAccelLimit) { 
limitFlag = FALSE; 
cerr << '\n imu y accel out of limits\n"; 

} 

if (fabs(newSample.sample[2]) > zAccelLimit) { 
limitFlag = FALSE; 
cerr << '\n imu z accel out of limits\n"; 


if (fabs(newSample.sample[3}) >rateLimit) { 
limitFlag = FALSE; 
cerr << "\n imu p rate out of limits\n"; 

} 

if (fabs(newSample.sample[4]) >rateLimit) { 
limitFlag = FALSE; 
cerr << '\n imu g rate out of limits\n"; 


} 
if (fabs(newSample.sample[5]) >rateLimit) { 


limitFlag = FALSE; 
cerr << '\n imu r rate out of limits\n"; 


} 

if (fabs(newSample.sample[6]) > speedLimit) { 
limitFlag = FALSE; 
cerr << '\n water speed out of limits\n"; 


} 


if (fabs(newSample.sample[7]) > headingLimit) { 
limitFlag = FALSE; 
cerr << '\n heading out of limits\n"; 


} 


return limitFlag; 


} 
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APPENDIX B: Serial Port Communication Source Code (C**) 


A. GLOBALS.H 


#ifndef _GLOBALS_H 
#define _GLOBALS_H 


// types 

typedef unsigned charBYTE; 
typedef unsigned short WORD; 
typedef unsigned long DWORD; 


#define MEM(seg,ofs) (*((BYTE far*)MK_FP(seg,ofs))) 
#define MEMW(seg,ofs) (*((WORD far*)MK_FP(seg,ofs))) 


enum Boolean {FALSE, TRUE}; 


// basic bit twiddles 

#define set(bit) (1<<bit) 

#define setb(data,bit) data | set(bit) 
#define clrb(data,bit) data & !set(bit) 
#define setbit(data,bit) data = setb(data,bit) 
#define clrbit(data,bit) data = clrb(data,bit) 


// specific to ports 
#define setportbit(reg,bit) outportb(reg,setb(inportb(reg),bit)) 
#define clrportbit(reg,bit) outportb(reg,clrb(Gnportb(reg), bit)) 


#endif 


B. BUFFER.H 


#ifndef BYTEBUFFER_H 
#define_ _BYTEBUFFER_H 


#include "towtypes.h" 


#define BYTEBUFSIZE 32 
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CLASS:Buffer 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: 

Base class for use as a polymorphic reference in the serial port code 


which defines a buffer to be used in serial port communications. 
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class Buffer { 
public: 


_ //Constructor 
Buffer(WORD sz) : getPtr(O), putPtr(Q), size(sz) {} 


//Checks for the arrival of new characters in the buffer 
virtual Boolean hasData() __{ return Boolean(putPtr != getPtr); } 


//How much of the Buffer is used (rounded percentage 0 - 100) 
virtualint capacityUsedQ); 

/fRead from the buffer 

virtual Boolean Get(BYTE&) = 0; 

/fRead to the buffer 

virtual void Add(BYTE) = 0; 


protected: 


//Increment the pointer to next position 
void inc(WORD& index) 
{ if (++index == size) index = 0; } 


//Decrement the pointer 
WORD before(WORD index) 
{ return (index == Q) ? size - 1 : index - 1);} 


WORD _ getPtr, //Location of unread data 


putPtr, //Location to read data to 
size; //Size of the buffer in bytes 
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Defines a single buffer of a specified size for buffering charaters 


received via serial port. 
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class byteBuffer : protected Buffer { 


public: 


byteBuffer(B YTE sz=B YTEBUFSIZE); 
~byteBufferQ { delete [} buf; } 


Buffer::hasData; 


// 
Buffer: :capacity Used; 


// buffer extraction 
Boolean Get(BYTE&); 


// buffer insertion 

void Add(BYTE ch); 

void Add(const char*); 

byteBuffer& operator+=(BYTEch) { Add(ch); return *this; } 


protected: 


}; 


#endif 


BYTE* _ buf; 


C. BUFFER.CPP 


#include <iostream.h> 
#include <stdio.h> 


#include "globals.h" 
#include "packbuff.h" 
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// Buffer 
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// Returns the percentage of the buffer in use 


int 
Buffer::capacityUsed() 
{ 


int cap = (putPtr + size) % size ~ getPtr; 
return 100 * cap / size; 


} 
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// byteBuffer 
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//Constructor, instantiates a buffer 
byteBuffer::byteBuffer(BYTE sz) : Buffer(sz) 
{ 


buf = new BYTE{size]; 
} 


//Reads a charcter from the buffer 


Boolean 
byteBuffer::Get(B YTE& data) 


if (hasData()) { 
data = buf[getPtr]; 
inc(getPtr); 
return TRUE; 


} 
return FALSE; 
} 


//Writes a character to the buffer and checks for buffer overflow 


void 
byteBuffer::Add(BYTE ch) 


buf[putPtr] = ch; 
inc(putPtr); 
if (thasData()) { //if there's no data after adding data, it overflowed 
cerr << '\nError: byteBuffer overflow\n"; 
} 
} 
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/{Writes a character to the buffer 


void 
byteBuffer:: Add(const char* s) 
{ 
while (*s) 
Add(*s++); 
} 


D. PACKBUFF.H 


#ifndef PACKBUFF_H 
#define PACKBUFF_H 


#include "buffer.h" 
#include "towtypes.h" 


#define PACKETBLOCKS 10 

#define SOH 0x01 

#define NAK 0x15 

#define ACK 0x06 

#define EndofFile Oxla 

#define EOT OX04 

#define SecsToTicks(x) ((long)(x * 182L) / 10L) 


class bufferedSerialPort; 
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CLASS:packetBuffer 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: 
Class buffers packets of data received via a modified XMODEM protocol. 
Uses a multiple buffer system in which each buffer is capable of 
holding a single packet of information. Buffers are filled and processed 
sequentially in a round robin fashion. Packets are checked for validity 


only upon attempted reads from the buffer. 
Pts OAS ANG he Ae Ae oR ae ARB AAC A 2h a as AC 282 a8 a AR 2s Ae Ie eS TTS He he Ne NOR eS POR De Ie OR ASD eA NS eRe eter Ae 


class packetBuffer : public Buffer { 
public: 


packetBuffer(bufferedSerialPort&, BYTE packetBlocks=PACKETBLOCKS); 
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~packetBuffer(){delete [] block; } 


Boolean hasData(); // check a block for valid packet 

Boolean Get(BYTE&) {return FALSE; } //Satifies inheritence requirements 
Boolean Get(PACKET); // get a complete structure filled in 

void Add(BYTE ch); // build a packet as each byte is added 


protected: 


Boolean validPacket(PACKET)); //check the checksum of a packet 
Boolean timeOut(); //check for an event time out 


bufferedSerialPort& port; //com port to ACK and Nak acknowledgements 


PACKET *block; // hold the buffered sample packet 
WORD _ current, // the current block in use 
last; // the last block in use 


BYTE  byteIndex; // place to put the next charater received 
long waitTime; // time waiting since last time out 
Boolean arriveFlag; //indicates if charater arrived since last check 


}; 


#endif 


E. PACKBUFF.CPP 


#include <iostream.h> 
#include <stdio.h> 
#include "serial.h" 
#include "packbuff.h" 


[ [SERRA EEEAALE EN ERELEL AERA LE EE EERE PR EEE 


// packetBuffer 


J PELE ELALEE EEE EEE EEE EEE EEE Se ee en eo 
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PROGRAM: Packet Buffer Constructor 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Constructor, Sets up number of buffers. Sets pointers to write into 
the buffers. Sends first NAK to start XMODEM protocol. 

CALLS:send (serialPort) 
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packetBuffer::packetB uffer(bufferedSerialPort& p, BYTE packetBlocks) : 
port(p), current(0), last(O), byteIndex(0), waitTime(SecsToTicks(10)), 
arriveFlag(FALSE), Buffer(packetBlocks) 

{ 
block = new PACKET[packetBlocks]; 
port.Send(NAK); 
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PROGRAM: Add 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Reads a single character into the buffer array. Check for 
end of a packet before moving to next buffer. Sends ACK and NAK following 
each Packet. 
RETURNS: void 
CALLED BY: interupt driven 
CALLS: none 
ae hy is Me ee Me a ae As A oer 2 oe a shah He obs oh ae oe He aE a A oes As Ae a A os aso a In a es a A de hea Me Re Ae An che He Ae ico ARI es ae pe As ake teh he oe eee ie A 
void 
packetBuffer::Add(BYTE data) { 


static Boolean EOTflag = FALSE; 
if (data == EOT & & EOTflag) { 


port.Send(ACK); //Acknowledge End of transmission character 
port.Send(NAK); //Request next packet 


if (EOTflag && (data == SOH)) { 


//setup for the start of a packet 
last = current; 
inc(current); 
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byteIndex = 0; 
EOTflag = FALSE; 
j 


arriveFlag = TRUE; 
block[current][byteIndex++] = data; //write character to buffer 


if (byteIndex == 131) { 
port.Send(ACK);  //Acknowledge packet 
waitTime = SecsToTicks(1L); //Reset wait time to one second 
EOTflag = TRUE; //Start waiting for the EOT character 


j 
} 
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PROGRAM: Get 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Reads a packet from the buffer into the input argument 
RETURNS: TRUE, if a packet was read 
CALLED BY: getSample (sampler) 
CALLS: — timeOut (packBuf) 
hasData (packBuf) 
inc (packBuf) 
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Boolean 
packetBuffer::Get(GPSdata data) 


{ 
if (arriveFlag) { 


arriveFlag = FALSE;//Reset flag to indicate new character arrivals 


if (hasDataQ) { 
memcpy (data, block[last], PACKETSIZE); 
last = current; 
return TRUE; //valid packet received 
} 
else { 
return FALSE; 


} 
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else { 
if (timeOut()) { 


// Possible race condition if characters arrive during these stmt 
// Assume lost character go to beginning of next buffer 
byteIndex = 0; 

inc(current); 


port.Send(NAK); //indicate lack timeout to sender 
waitTime = SecsToTicks(1L); —_//set wait time to one second 


} 
return FALSE; 
} 


[HRA RAAT AAAI HR ARH RH RRR KAR ATA KA AA RAHA AH AK ARH AAA R HR ARIA EH EAR EEE 
PROGRAM: hasData 
AUTHOR: Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Determine if a valid new packet has arrived. 
RETURNS: TRUE, if a new unread packet is in a buffer 
CALLED BY:Get (packbuff) 
CALLS: _ validPacket 
Boolean 
packetBuffer: :hasData() 


if (last != current) { 
if (validPacket(block[last])) { 
return TRUE; //New valid packet available 


} 
else { 
last = current; 
return FALSE; 
} 
} 
else { 
return FALSE; 
} 


} 
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PROGRAM: validPacket 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Checks a packet for sequence number and complement and 
checks for valid checksum. 
RETURNS: TRUE if the packet is valid 
CALLED BY:hasData (packBuff) 
CALLS: none 
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Boolean 
packetBuffer::validPacket(PACKET packet) 
{ 
BYTE sum(Q); 


if (char(packet[1]) == ~char(packet[2])) { 


for (int 1 = 3;1< 131; i++) { 
sum += packetfi]; 


} 


if (sum == packet[131]) { 
// cerr << " valid packet" << endi; 
return TRUE; 
} 


else { 
cerr << "invalid packet checksum " << endl; 
return FALSE; 


} 


cerr << "invalid packet sequence number" << endl; 
return FALSE; 


} 
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PROGRAM:timeOut 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Determine if a time out has occured since the last character 
was received by checking the wait time slot. 

RETURNS: TRUE, if a time out has occured 

CALLED BY:Get 

CALLS: none 
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Boolean 
packetBuffer::timeOut() 
{ 
static unsigned long far *timer = (unsigned long far *) MK_FP(0x40, Ox6c), 
currentTime, 
lastTime; 


currentTime = *timer;// Get the current time. 


if (waitTime >0) { =// New wait period? 
waitTime *= -1; // Go negative, count up 
lastTime = currentTime; //Save time to later determine time delta 


} 


if (currentTime != lastTime) { // Has time passed? 
if (currentTime <= lastTime) { 


wait Timet++; 
} 
else { 

waitTime += (unsigned int) (currentTime - lastTime); 
} 


lastTime = currentTime; //Save time to later determine time delta 


} 


return Boolean(waitTime >= OL); // Return TRUE is zero has been reached 


} 
F. GPSBUFF.H 


#ifndef _GPSBUFF_H 
#define _ GPSBUFF_H 
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#include "buffer.h" 
#include "towtypes.h" 


#define GPSBLOCKS 4 
#define LINE_FEED 10 





[2 *C HHA A ACR HH AR AHHH AR BH HR RAHM AR BAA ACA HRB BIHAR EHH HH EE EH HH 


Class buffers GPS position messages via serial port communications. 
Uses a multiple buffer system in which each buffer is capable of 
holding a single position message. Buffers are filled and processed 
sequentially in a round robin fashion. Messages are checked for validity 
only upon attempted reads from the buffer. 
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class GPSbuffer : public Buffer { 
public: 


GPSbuffer(B YTE GPSblocks=GPSBLOCKS); 
~GPSbuffer() {delete [] block; } 


Boolean hasData(); // a complete structure is ready 

Boolean Get(BYTE&) {return FALSE; } 

Boolean Get(GPSdata); // get a complete structure filled in 

void Add(BYTE ch); // build the structure as each byte is added 


protected: 
Boolean validHeader(GPSdata); // check a block for valid header 
GPSdata *block; //hold the buffered GPS data 
WORD current, // the current GPS block in use 
last; // the last GPS block in use 


BYTE *putPlace; // place to put the next charater received 


}; 


#endif 
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G. GPSBUFF.CPP 


#include <iostream.h> 
#include <stdio.h> 


#include "gpsbuff.h" 


] PERERA EEEE ES EEE EEE EEE EEE EEA EEE EE RIA IEE RET 

// GPSbuffer 

[fEEEEEEREERLELELELEL EEE EERE ERED ER REET 
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PROGRAM:GPSbuffer (Constructor) 

AUTHOR:Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: 
Allocates message buffers, indicate that no data has been 
received by equalizing current and last and set position 

into which initial character will be read. 

RETURNS:nothing. 

CALLED BY:navigator class (nav.h) 

CALLS:none. 
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GPSbuffer::GPSbuffer(B YTE GPSblocks) : 
current(Q), last(0), Buffer(GPSblocks) 

{ 
block = new GPSdata[GPSblocks]; 
putPlace = &(block({current][0]); 

} 


[BARR A AAA AR A MAAR ACH A AHH RAR TEA A TIER I IR SOR AIC RR TCI A TR I IR A I 
PROGRAM:GPSbuffer::Add 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: 
Interrupt driven routine which writes incoming characters into 
into the gps buffers 
RETURNS :nothing. 
CALLED BY: interupt driven by bufferedSerialPort 
CALLS:none. 
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— void 
GPSbuffer::Add(BYTE data){ 
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static Boolean IfFlag = FALSE; 


//Is anew message starting? 
if (IfFlag && (data == '@')) { 


last = current; // Set last to buffer with newest message. 
inc(current); // Set current to the next buffer 
// Set putPlace to the beginning of the next buffer. 
putPlace = &(block{current][0]); 
ifFlag = FALSE; // reset for end of next message. 

} 


*putPlace++ = data;// Write character into the buffer. 


//Has the end of a message been received? 
if (data == LINE_FEED) { 
IfFlag = TRUE; 
} 
} 
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PROGRAM:GPSbuffer::Get 
AUTHOR: Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: 
Checks to see if a new message has arrived, copies it into the 
input argument data and returns a flag to indicate whether a new 
message was received. 
RETURNS:TRUE, if a new valid position has been received. 
FALSE, otherwise 
CALLED BY:navPosit (nav.cpp) 
initializeNavigator (nav.cpp) 
CALLS:GPSbuffer::hasData 
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Boolean 
GPSbuffer::Get(GPSdata data) 


// Has a new valid message been received. 
if (hasData()) { 
// Copy the message out of the buffer. 
memcpy (data, block + last, GPSBLOCKSIZE); 
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// Indicate that this message has been read. 
last = current; 


retum TRUE; 
} 
else { 

return FALSE; 
} 


} 


[RRR RAK AAR RR RRA HARRAH AR HR A RH RK A AR HH HR RK EK 


PROGRAM:GPSbuffer::hasData 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: 
_ Determines whether a new message has been received and checks 
to see if it has a valid header. 
RETURNS:TRUE, if a new valid message has been received. 
CALLED BY: GPSbuffer::Get (buffer.cpp) 
CALLS: _ validHeader (buffer.cpp) 
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Boolean 
GPSbuffer::hasDataQ) 


if (last != current) && (validHeader(block[last]))) { 


return TRUE; 
} 
else { 

return FALSE; 
} 
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PROGRAM: validHeader 
AUTHOR:Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: 
Checks to see if a message has the proper header for a Motorola 
position message. (@@Ba) 
RETURNS: TRUE, if the header is valid. FALSE, otherwise. 
CALLED BY:GPSbuffer::hasData (buffer.cpp) 
CALLS:none. 
COMMENTS: 
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Boolean 
GPSbuffer::validHeader(GPSdata dataPtr) 
{ 
if ((dataPtr[O] == '@') && 
(dataPtr[1] == '@') && 
(dataPtr[2] == 'B') && 
(dataPtr[3] == 'a')) { 
return TRUE; 
} 
else { 
return FALSE; 
} 
} 


H. PORTBANK.H 


#ifndef PORTBANK_H 
#define_ PORTBANK_H 


#include "serial.h" 
#include "buffer.h" 
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CLASS:portBank 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 

FUNCTION: Manages up to four bufferdSerialPort instances. 
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class portBank { 
public: 


portBank(); 

~portBank() { cleanup(); } 

bufferedSerialPort& Init(COMport portnum, BYTE irg, BaudRate, ParityType, 
BYTE wordlen, BYTE stopbits, handShake, Bufferdé); 

void cleanup(); 


friend IntHandlerT ype COM thandler, COM2handler, COM3handler, COM4handler; 


protected: 
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bufferedSerialPort* ports[4}; 
: 
extern portBank COMports; 


#endif 


I. PORTBANK.CPP 


#include <iostream.h> 


#include "serial.h" 
#include "buffer.h" 
#include "portbank.h" 


portBank COMports; 
// Constructor, sets up array of ports 


portBank::portBank() 
{ 
for (int 1 = Q; 1< 4; 1++) 
ports[i] = 0; 
} 


// Resets all ports to the original parameters 


void 
portBank::cleanup() 
{ 


for (int i=0; 1<4; 1++) 
delete ports[i]; 
} 


// Initializes a serial port based up the input arguments 


bufferedSerialPort& 


portBank: :Init(COMport portnum, BYTE irg, BaudRate baud, ParityType parity, 
BYTE wordlen, BYTE stopbits, handShake shake, Buffer& buf) 


{ 
int index = BYTE(portnum) - 1; 
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if (ports[index]) 
delete ports[index]; 
ports[index] = new bufferedSerialPort(portnum, irq, baud, parity, 
wordlen, stopbits, shake, buf); 
return *ports[index]; 


} 


// Three specific interrupt handlers which map each interupt to the 
// proper ISR. 


void 

interrupt 

COM thandler(...) 

{ 
COMports.ports[0]->processInterrupt(); 
EOI, 

} 


void 
interrupt 
COM?2handler‘...) 


COMports.ports[1]->processInterrupt(); 
EOI, 
} 


void 

interrupt 

COM3handler(...) 

{ 
COMports.ports[2]->processInterrupt(); 
EOI; 

} 


void 

interrupt 

COM4handler(...) 

{ 
COMports.ports[3]->processInterrupt(); 
EOI; 

} 
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J. SERJAL.H 


#ifndef SERIAL H 
#define SERIAL H 


#include <dos.h> 
#include <stdio.h> 
#include "globals.h" 
#include "buffer.h" 


// user defines 
#define ALMOST FULL 80 //% full to tum off DTR 


// leave the following alone - hardware specific 


enum COMport {COM1=1, COM2, COM3, COM4}; 
enum BaudRate {b300, b1200, b2400, b4800, b9600}; 
enum ParityType {ERROR=-1, NOPARITY, ODD, EVEN}; 
enum handShake {NONE, RTS_CTS, XON_XOFF}; 

enum Shake {off, on}; 

enum interruptT ype {rx_rdy, tx_rdy, line_stat, modem_stat}; 


#define BIOSMEMSEG 0x40 
#define DLAB 0x80 

#define IRQPORT 0x21 

#define EOI outportb(0x20,0x20) 


#define COM1lbase MEMW(BIOSMEMSEG.,0) 
#define COM2basee MEMW(BIOSMEMSEG,2) 
#define COM3base Ox03e8 
#define COM4base Ox02e8 


#define TX (portBase) 
#define RX (portBase) 
#define IER (portBase+1) 
#define IIR (portBase+2) 
#define LCR (portBase+3) 
#define MCR (portBase+4) 
#define LSR (portBase+5) 
#define MSR (portBase+6) 
#define LO_LATCH  (portBase) 
#define HI_LATCH (portBase+1) 
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CLASS:serialPort 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 

FUNCTION: Defines a simple serial port. 
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class serialPort { 
public: 


serialPort(COMport port, BaudRate, ParityType, BYTE wordlen, 
BYTE stopbits, handShake); 


Boolean Send(BYTE data); 
Boolean Get(BYTE& data); 


inline Boolean dataReady(Q); 
Boolean statusChanged() 
{ return Boolean((Gfportbit(MSR,0) Il ifportbit(MSR,1))); } 


// the rest are only if handshake is specified as RTS_CTS 


Boolean isCTSonQ { return ifportbit(MSR,4); } 
Boolean isDSRonQ) { return ifportbit(MSR,5); } 
void setDTRon() { setportbit(MCR,0); } 


void setD TRoff() { clrportbit(MCR,0); } 
void toggleDTRQ; 


void setRTSon() { setportbit(MCR, 1); } 
void setR TSoff() { clrportbit(MCR,1); } 
void toggleRTSQ; 


protected: 
WORD portBase; 
handShake ShakeType; 
Shake DTRstate, 
RTSstate; 


inline Boolean ifportbit(WORD, BYTE); 
inline void toggle(Shake&); 
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// this is the type for a standard interrupt handler 
typedef void interrupt (IntHandlerType)(...); 


[HRA AHH AIHA A A AH RHR HR BA AH RA KAA KAA A ER I ET ETE HE AE 


CLASS:bufferedSerialPort 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: Defines a buffered serial port which is interrupt driven 


on receive, and buffers all incoming characters in the specified buffer 
2 2 2 2 AR 2 a a a A 2 A A KH HK ACA HH A A A He A ACK Ae A HA IR I HR KA KH A A A A A EA ACE A He A KE EK / 


class bufferedSerialPort : public serialPort { 


public: 
bufferedSerialPort(COMport portnum, BYTE irq, BaudRate, ParityType, 
BYTE wordlen, BYTE stopbits, handShake, 
Buffer& ); 
~bufferedSerialPort(); 
Boolean Get(BYTE& data); // buffered version 
protected: 


Buffer& buf; 

BYTE irgbit, //Value to allow enable PIC interrupts for COM port 
origirg, //keep the original value of the 8259 mask register 
comint; 

void processInterrupt(); // buffers the incoming character 

IntHandlerType *origcomint; // keep the original vector for restoring later 

// this allows the actual handlers to access processInterrupt() 


friend IntHandlerType COMIhandler, COM2handier, COM3handler, 
COM4handler; 
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}; 


#endif 


K. SERIAL.CPP 


#include <iostream.h> 
#include <stdio.h> 
#include "serial.h" 


[18H Ae He Hee Ae A EA EH HR EEE EERE EE ERE ERK AHR E RB EE E 


Usage Note: Because of the interrupt handlers used, you MUST call 
your bufferedSerialPort objects portl, port2, or port3, so the 


right handler gets called and can properly service the interrupt. 
ake He 3 2 He 6 2K ae he 2 he He He eK He A HI A A He A A A A I RR HK RR RK AK HK HK IK HR KK HK KA ACK HH | 
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PROGRAM: serialPort (Constructor) 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: 

Initializes the one of the Serial Ports. 

1) Determines the base I/O port address for the given COM port 
2) Sets the 8259 IRQ mask value 
3) Initializes the port parameters - baud, parity, etc. 
4) Calls the routine to initialize interrupt handling 
5) Enables DTR and RTS, indicating ready to go 
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serialPort::serialPort(COMport port, BaudRate speed, ParityType parity, 
BYTE wordlen, BYTE stopbits, handShake hs) : 
DTRstate(off), RTSstate(off), ShakeType(hs) 
{ 
switch (port) { 
case COMI: portBase = COM |base; 
break; 
case COM2: portBase = COM2base; 
break; 
case COM3: portBase = COM3base; 
break; 
case COM4: portBase = COM4base; 
break; 
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} //switch 


const WORD _ bauddiv[{] = {0x180, 0x60, 0x30, 0x18, OxC}; 
// Change 1 

outportb(IER,0); = // disable UART interrupts 

(void)inportb(LSR); 

(void)inportb(MSR); 

(void)inportb(IIR); 

(void)inportb(RX); 

outportb(LCR,DLAB); //set DLAB so can set baud rate (read only port) 

outportb(LO_LATCH,bauddiv[speed] & OxFF); 

outportb(HI LATCH, (bauddiv[speed] & OxFFOQ) >> 8); 

setportbit(MCR,3); //tarn OUT2 on 


BYTE opt =0; 
if (parity != NOPARITY) { 
setbit(opt,3); _ // enable parity 
if (parity == EVEN) // set even parity bit. if odd, leave bit 0 
setbit(opt,4); 


// now set the word length. len of 5 sets both bits 0 and 1 to 
// 0, 6 sets to 01, 7 to 10 and 8 to 11 

opt I= wordlen-5; 

opt |= --stopbits << 2; 

outportb(LCR, opt); 


if (ShakeType == RTS_CTS) { 
setDTRonQ; 
setRTSon(); 
} 
} 
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PROGRAM: Get 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: 
Gets a byte from the port. Returns true if there's one there, and fills 
in the byte parameter. If theres no character, the parameter 1s left 


alone, and false is returned 
2 2e ae 6 2 he ee A 2 ae ie 2 2 2 ee 2B He 2 He A He A AK He He He 2 He HH He He Ae He 6 2 ae He He 8 eA ee ee A ee ee ie A ae EK 
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Boolean 
serialPort::Get(B YTE& data) 


if (dataReady()) { //make sure there's a char there 
data = inportb(RX); //read character from 8250 
return TRUE; 
} 
else 
return FALSE; 
} 


[HERRERA RAR AAA ERE EEE ELA EAHA EB NAK EER A EE RA EERE EEE REE EEE EEE 


PROGRAM: Send 
AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: 
Outputs a single character to the port. Returns Boolean 


status indicating whether successful 
2 2 2 a A 2 a 2K 2 2 2 a A ee 6 ae I ee A A a AK 2 ae A A 2 He He He HK HK He A HB A A A A HK A A A KK EE / 


Boolean 
serialPort::Send(B Y TE data) 


while (!(ifportbit(LSR,5))) —_// wait until THR ready 
- // NULL statement 


switch (ShakeType) { 
case NONE: _ outportb(TX,data); 
return TRUE; 
case RTS_CTS: if (isCTSon( && isDSRonQ) { 
outportb(TX,data); 
return TRUE; 
} 
else return FALSE; 
case XON_XOFF: 


default: 
// add this later if needed 
break; 
} 
return FALSE; 


} 
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[RRR ABA AAR RHA AA RAAB BAA AAR BAH I HH AB RA A RR I RI RE HK EK 


PROGRAM: dataReady 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 

FUNCTION: Checks port to see if a character has arrived. 


He 2 2 2 9 A 2K 2 Ke hee he fe 2 2 A a 2 A A A A AE A A A A A A A A AK AK I Ke HA AK A A ER A A A A HH KA EE | 


inline 
Boolean 
serialPort::dataReady() 
{ 
/* 
if Gfportbit(LSR,1)) { 
cerr <<'\nOverrun Error\n"; 


} 
if (ifportbit(LSR,2)) { 
cerr <<'\nParity Error\n"; 


} 
if (fportbit(LSR,3)) { 
cerr <<"\nFraming Error\n"; 
} 
a 
return ifportbit(LSR,0); 
} 


[RRRRAAK HAKKAR RARE EAE EAR AAA RRR EK EAE EE HERB BE HERE RE ER A 


PROGRAM: ifportbit 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 

FUNCTION: 


2H 2 HH He AE A A He A AR HK A He A HK He eR A A HO A RK A HK HH A A HK AE / 


inline 
Boolean 
serialPort::ifportbit(WORD reg, BYTE bit) 
{ 
BYTE on = inportb(teg); 
on &= set(bit); 
return Boolean(on == set(bit)); 


} 
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[RRARAAH AAA AAAI ARH RA RAK EKA ARR AA AA AAR ARR HRI ERA RAE AA AA ERE EEE 


PROGRAM: toggleDTR 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: toggles Data Transmit Ready if RTS_CTS is off 


He RR AK a AK He 2 aR a A A EK HR A HK RI IK I OK A AR IK AK A AK A HE AK HK EK RK HK KE EK I  / 


void 
serialPort::toggleDTRQ) 


if (ShakeType != RTS_CTS) 
return; | 

if (DTRstate == off) 
setDTRon(Q); 

else 
setDTRoffQ; 

toggle(DTRstate); 

} 


[HRA AAA AR RRA ERIK RAR AA AAA EAN BH AEE RH KARE EH IER TE EERE EE 
PROGRAM: toggleRTS 
AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: toggle Ready to Send (RTS) if RTS_CTS is on. 


2K HC He a A He He He a A A A A A A He a He a A He a A AK He A CE Ae He HE A A AK I A AK A A KK HK A A EK A A HE A A KK EE / 


void 
serialPort::toggleRTSQ 


if (ShakeType != RTS_CTS) 
return; 

if (RTSstate == off) 
setRTSonQ); 

else 
setRTSoffQ; 

toggle(RTS state); 

} 
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[EEEEELELALEREEEREELS FEEELELLELE LEER L ES ERA EEE REC EEE CE eee ee eee ee 


PROGRAM: toggle 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 | 

FUNCTION: toggles value of the input variable 


2k He 2 2 2 2 2 ee he 2s he 28 2h 2 2 2h 26 2 Ae 2 ae he 2 ee He Ae A 2 2 a A A He he Ae 2 he he ee 2 2 2 ae ee 2 2 2 2 6 A I 2 2 2 A RK OE 2K | 


inline 
void 
serialPort::toggle(Shake& h) 


if (h == off) 
h =on; 
else 
h = off; 
} 


f [PEL AEAAEA LEE EE EAE ERE EES EEE EEE OE LETC Ee ee 


// bufferedSerialPort 


/ ok 2k 3k 2k 2k 2k 2 2k 2k 2 ok 2k 2k ok ok 2k 2k Ok 2K 2 kK ok Ko OE KR KK KR KKK KKK KKK KK KKK KK KKK KKK KK KK 


[PEEEEEEEEEEE REESE EEE A EE RE ee Re Re ACE RE eh ee ee ea Ae A a ee ee 


PROGRAM: bufferedSerialPort (Constructor) 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 

DATE:11 July 1995 

FUNCTION: 

Initializes the interrupts for the Serial Port. 

1) takes over the original COM interrupt 
2) sets the port bits, parity, and stop bit 
3) enables interrupts on the 8250 (async chip) 
4) enables the async interrupt on the 8259 PIC 


He A Ae Ae i ie eae ee a ie eee ea he Ae he He Ae Ae ae AG A ele ae a ee Ae A A a Ae ae ae a Soe ee oe eee eee Ar hehe eerie eae: | 


bufferedSerialPort::bufferedSerialPort(COMport portnum, BYTE irq, 
BaudRate baud, ParityType parity, BYTE wordlen, 
BYTE stopbits, handShake hs, Buffer& b) 
: serialPort(portnum, baud, parity, wordlen, stopbits, hs), 
buf(b), irqbitGrq), comint(irgbit+8) 


{ 
if (ShakeType == RTS_CTS) { // turn it off first, because it was enabled 
setDTRoffQ; // in the base class 
setRTSoffQ; 
} 
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origcomint = getvect(comint); _ //remember the original vector 


switch (portnum) { 


case COM1: 
setvect(comint,COM Ihandler); //point to the new handler 
break; 
case COM2: 
setvect(comint, COM2handler); //point to the new handler 
break; 
case COM3: 
setvect(comint, COM3handler); //point to the new handler 
break; 
case COMA: 
setvect(comint, COM4handler); //point to the new handler 
break; 
} 
// setportbit(MCR,3); //tarn OUT2 on 
disable(); _ // disable all interrupts - critical section 
setportbit(IER,rx_rdy); //enable ints on receive only 


origirg = inportb(IRQPORT); //remember how it was 
clrportbitIRQPORT, irgbit); //enable COM ints 


if (ShakeType == RTS_CTS) { 


setDTRonQ); 
setRTSonQ); 
} 
enable(); 
EO!I; 
} 


[RERRBE HARA AARA EEA EER ERE ERA EERE IEE EERIE ERA ER ERE EEE REE EERE E EERE 


PROGRAM: ~bufferedSerialPort 
AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: 
Resets the interrupts. 1) disables the 8250 (async chip) 
2) disables the interrupt chip for async int 
3) resets the 8259 PIC 


2 oe oe oe 2s 2 ee 2 oe oi 2 2 He 2 eA HK Hee a Be EG ER He He He he he Ae he He ee ee A 2H 2 ee he he He He Ae He 2 2 He Ae 2 2 Ae A I / 
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bufferedSerialPort::~bufferedSerialPort() 

{ 
setvect(comint,origcomint); //set the interrupt vector back 
outportb(IER,0); //disable further UART interrupts 
outportb(MCR,0); //tarn everything off 
outportb(IRQPORT, origirq); 
EOI; 

} 


[HRA AAR AA A ARR HRA KH RAK AA KAA AAA RAK MH RHE RHR HR A KR EEE EE 
PROGRAM: Get 
AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Calls Get base on buffer type 


ake ae 9 9 2K oe 2 2 2k ee 2B 2 a 2 ae a 2 He Ae HE AC AC HG he EE 2 He He ee 8 Ee eK A 3 2 9 2 2 oe he ee 2K 3 2 3 kK 2 KK RAK A HE / 


Boolean 
bufferedSerialPort::Get(B YTE& data) 
{ 
return buf.Get(data); 
} 


JRERERLEEEL EEL LAAELEEELEAEEAEEEE LESSEE LE LE EEL EE ALE TERRELL ERLE A EES 
PROGRAM: processInterupt 
AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Calls the ISR based upon buffer type 


ake 3k ae fe 2h 2 2K 2 he 2 2 2 he 2 ae He 2 he A 2 a 2 ee 2 He A He A 2 He ae 2 He He A A HH HK He A AR He HE He A EH EK 


void 
bufferedSerialPort::processInterrupt() 


if (dataReady()) { //make sure there's a char there 
BYTE data = inportb(RX); //read character from 8250 


buf.Add(data); 
if (ShakeType == RTS_CTS && buf.capacityUsedQ > ALMOST_FULL) 
setDTRoffQ; 
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[HRB RRR RRA AKA REE AA EERE AR BBA AH AH HH HHH AHH BBB RH EE I 
PROGRAM: showPorts 
AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE:11 July 1995 
FUNCTION: Prints interupt vector addresses 


He 2 2 ae 2 ae ee hs fe ie 2 2k ee ee ae A 2 oe a oO OR A OK AR A AR GK A AK eK HR IK EK He ER EE I EE KE | 


int 
showPorts() 


BYTE* p = (BYTE*)COM2base; 

p t+=5; 

fprintf(stderr,"%X ",*p++); 

fprintf(stderr,"% X\n",*p++); 
fprintf(stderr,"IRQPORT = %X", inportbURQPORT)); 
return Q; 


} 
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APPENDIX C: Navigation Simulation Source Code (LISP) 


; OOD, Engineer, and Weapons classes written by Brad Leonhardt (Fall 94) 
; Written for CS4314 final project 


A. OOD.CL 


a ENGINEER CLASS AND METHODS -------------------------- 
(defclass engineerclass()(Q) ) 


(defmethod order((engineer engineerclass)string) 
(write-string " ENG --> EOOW : ") 
(write string) 

(write-line "!"')) 


(defmethod query((engineer engineerclass)string) 
(write-string " ENG --> EOOW : ") 
(write string) 

(write-string "’") 
(write-string " ") 
(member (read) *affirmative*)) 


eee eee WEAPONS OFFICER CLASS AND METHODS-------------------=- 


(defclass weapons-officerclass()Q) ) 


(defmethod order((weapons-officer weapons-officerclass)string) 
(write-string " WEPS --> SONAR: ") 
(write string) 
(write-line "!"')) 


(defmethod query((weapons-officer weapons-officerclass)string) 
(write-string " WEPS --> SONAR: ") 
(write string) 
(write-string "?") 
(write-string " ") 
(member (read) *affirmative*)) 
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ee OOD CLASS AND METHODS ------------------------------- 
(defclass oodclass () 


( (engineer 
‘initform (make-instance ‘engineerclass) 
saccessor engineer) 
(navigator 
-initform (make-instance ‘navigatorclass) 
‘accessor navigator) 
(weapons-officer 
‘initform (make-instance 'weapons-officerclass) 
accessor weapons-officer) )) 


(defmethod command((ood oodclass) string person) 
(write-string " OOD -->") 
(write person) 
(write-string " :") 
(write string) 
(write-line "!") 
(order person string)) 


(defmethod ask ((ood oodclass) string person) 
(write-string " OOD -->") 
(write person) 
(write-string " :") 
(write string) 
(write-line "?") 
(query person string)) 


(setf *affirmative* '(1 t y)) 
(defmethod goto-wp ((ood oodclass)) 
(do* () 


((target-point-reached (navigator ood)) (order ood 'Get_next-waypoint)) 
(query ood 'Waypoint_reached))) 


(defmethod order((ood oodclass) order) 
(increment-time auv-clock) 
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(case order 
(Initialize_vehicle 
(and (command ood order (engineer ood)) 
(command ood order (navigator ood) ) 
(command ood order (weapons-officer ood)) )) 


(Transit_to_task_location 
(command ood order (navigator ood))) 


(Search_area_for_target 
(command ood order (weapons-officer ood))) 


(Commence_task_on_target 
(command ood order (weapons-officer ood))) 


(Transit_to_recovery_point 
(command ood order (navigator ood))) 


(Abort_mission 
(cond 
((ask ood ‘Is_recovery_point_obtainable (navigator ood)) 
(setf *current_phase* 8)) 
(t (command ood order (engineer ood))))) 


(Mission_complete 
(command ood order (engineer ood))) 


(Get_next-waypoint 
(command ood order (navigator 0od))) 


(Get_GPS_ fix 
(command ood order (navigator 0od))) 


(Loiter_and_Start_Global_Replanner 
(command ood order (navigator ood))) 


(Log_new_obstacle 
(command ood order (navigator ood))) 
(Loiter_and_Start_Local_Replanner 
(command ood order (navigator ood))) 


(Drop_package 
(command ood order (weapons-officer 00d))) 
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(t nil) )) 


‘alain query ((ood oodclass) question) 
(increment-time auv-clock) 
(case question 


(Critical_ Systems_OK 
(cond ((ask ood question (engineer ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Initialization_complete 
(cond ((and (ask ood question (weapons-officer ood)) 
(ask ood question (navigator ood)) 
(ask ood question (engineer ood)) 
(write-line "OOD -->CO: yes" )) t) 
(t (write-line "OOD -->CO: no")) )) 


(Initialization_aborted 
(cond ((and (not (ask ood question (weapons-officer ood))) 
(not (ask ood question (navigator ood))) 
(not (ask ood question (engineer ood))) 
(write-line "OOD -->CO: no" ))) 
(t (write-line "OOD -->CO: yes") t))) 


(Task_location_reached 
(cond ((ask ood question (navigator ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Target_found 
(cond ((ask ood question (weapons-officer ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(At_recovery_point 
(cond ((ask ood question (navigator ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line “OOD -->CO: no")) )) 
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(Waypoint_reached 
(cond ((ask ood question (navigator ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Waypoint_process_OK 
(cond ((ask ood question (navigator ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Got_next_waypoint 
(cond ((ask ood question (navigator ood)) 
(write-line “OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(GPS_fix_ needed 
(cond ((ask ood question (navigator ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line “OOD -->CO: no")) )) 


(GPS_fix_obtained 
(cond ((ask ood question (navigator ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Abort_GPS_ fix 
(cond ((ask ood question (navigator 0od)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(NonCritical_Systems_OK 
(cond ((and (ask ood question (weapons-officer ood)) 
(ask ood question (engineer ood)) ) 
(write-line “OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Area_clear_of_uncharted_obstacles 
(cond ((ask ood question (navigator ood)) 
(write-line “OOD -->CO: yes" ) t) 
(t (write-line "QOD -->CO: no")) )) 


(New_obstacle_logged 
(cond ((ask ood question (navigator o0od)) 
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(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Log_system_failure 
(cond ((ask ood question (navigator ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Setpoints_and_modes_sent 
(cond ((ask ood question (navigator ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Setpoints_and_modes_system_OK 
(cond ((ask ood question (navigator ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line “OOD -->CO: no")) )) 


(Search_pattern_completed 
(cond ((ask ood question (weapons-officer ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Sonar_failure 
(cond ((ask ood question (weapons-officer 00d)) 
(write-line "OOD -->CO: yes” ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Standoff_distance_reached 
(cond ((ask ood question (weapons-officer ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Abort_homing 
(cond ((ask ood question (weapons-officer ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(Is_package_dropped 
(cond ((ask ood question (weapons-officer 0o0d)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 
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(Is_package_drop_aborted 
(cond ((ask ood question (weapons-officer ood)) 
(write-line "OOD -->CO: yes" ) t) 
(t (write-line "OOD -->CO: no")) )) 


(t nil) )) 


B. NAVIGATOR.CL 


(setf auv-clock (make-instance ‘clock-class)) 


(setf auv (make-instance “sub-class)) 


a ee eee NAVIGATOR CLASS AND METHODS-----------------------+- 


(defclass navigatorclass() 
((replanner 
:initform (make-instance ‘replannerclass) 
accessor replanner) 
(gps-obtained-flag 
‘initform “no 
accessor gps-obtained-flag) 
(last-position 
‘initform ‘(0 0 0) 
accessor last-position) 
(target-way-point 
‘initform ‘(0 0 0 0) 
accessor target-way-point) 
(way-point-list 
‘initform ‘((0 -50 160 0)(0 -100 100 0)(0 -100 -100 0) 
(0 50 -50 0)(0 100 100 0)(0 150 150 Q)) 
accessor way-point-list) 
(sub-way-point-list 
sinitform () 
saccessor sub-way-point-list))) 


(defmethod update-position ((navigator navigatorclass)) 
(update-auv-position auv) ; causes real world position of auv to change 
(setf (last-position navigator) (list (first (posture auv)) 

(second (posture auv)) 
(third (posture auv)))) 
(if (target-point-reached navigator) (update-sub-way-point navigator))) 
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(defmethod update-sub-way-point ((navigator navigatorclass)) 
(with-slots(target-way-point sub-way-point-list) navigator 
(cond ((not (null sub-way-point-list)) 
(setf sub-way-point-list (rest sub-way-point-list)) 
(setf target-way-point (first sub-way-point-list)) 
(update-helm-commands (auto-pilot auv) 

(list (calculate-target-track navigator) 
(commanded-speed (auto-pilot auv)) 
(commanded-depth (auto-pilot auv)) 
(commanded-dive-angle (auto-pilot auv))))) 

(t (update-way-point navigator))))) 


(defmethod calculate-target-track ((navigator navigatorclass)) 
(let* ((course (radian-true-course (last-position navigator) 
(rest (target-way-point navigator))))) 
(list (second (target-way-point navigator)) 
(third (target-way-point navigator)) course))) 


(defmethod update-way-point ((navigator navigatorclass)) 
(with-slots (last-position target-way-point sub-way-point-list 
way-point-list) navigator 

(cond ((not(null way-point-list)) 

(setf sub-way-point-list (rest (plan-route 
(replanner navigator) world 
(append (list 0) last-position) 
(first way-point-list)))) 
(setf target-way-point (first sub-way-point-list)) 
(setf way-point-list (rest way-point-list))) 
(t (setf target-way-point “(0 0 0 0)))) 
(update-helm-commands (auto-pilot auv) 

(list (calculate-target-track navigator) 
(commanded-speed (auto-pilot auv)) 
(commanded-depth (auto-pilot auv)) 
(commanded-dive-angle (auto-pilot auv)))))) 


(defmethod get-recovery-point ((navigator navigatorclass)) 
(with-slots (target-way-point way-point-list) navigator 

(cond ((not(null way-point-list)) 

(setf target-way-point (last way-point-list)) 

(setf way-point-list nil)) 

(t (setf target-way-point ‘(0 0 0 0))))) 
(update-helm-commands (auto-pilot auv) 
(list (calculate-target-track navigator) 
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(commanded-speed (auto-pilot auv)) 
(commanded-depth (auto-pilot auv)) 
(commanded-dive-angle (auto-pilot auv))))) 


(defmethod target-point-reached ((navigator navigatorclass)) 
(with-slots (target-way-point last-position) navigator 
(if (> 10 (distance last-position (rest target-way-point))) t))) 


(defmethod get-gps-fix ((navigator navigatorclass)) 
(let* ((gps-fix (get-gps-fix (gps auv))) 
(gps-fix-time (current-time auv-clock))) 
(correct-position (ins auv) gps-fix gps-fix-time) 
(setf (last-position navigator) gps-fix)) 
(update-helm-commands (auto-pilot auv) 

(list (calculate-target-track navigator) 
(commanded-speed (auto-pilot auv)) 
(commanded-depth (auto-pilot auv)) 
(commanded-dive-angle (auto-pilot auv))))) 

(defmethod check-setpoints-modes ((navigator navigatorclass)) t) 


(setf *affirmative* ‘(1 t y)) 


‘defmethod order((navigator navigatorclass) order) 
(update-position navigator) 
(case order 
(Initialize_vehicle 


(initialize (imu auv)) 
(initialize (depth-cell auv)) 
(initialize (gps auv)) 
(initialize (uhf-receiver auv)) 
(initialize (auto-pilot auv))) 


(Transit_to_task_location  t) 


(Transit_to_recovery_point 
(get-recovery-point navigator) t) 


(Abort_mission 
(setf (power-status (imu auv)) ‘off) 


(setf (power-status (depth-cell auv)) ‘off) 
(setf (power-status (gps auv)) ‘off) 
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(setf (power-status (uhf-receiver auv)) “off) 
(setf (power-status (auto-pilot auv)) ‘off)) 


(Get_next-waypoint 
(update-way-point navigator) t) 


(Get_GPS_fix 
(setf (gps-obtained-flag navigator) “no) 
(get-gps-fix navigator) 
(setf (gps-obtained-flag navigator) “yes)) 


(Loiter_and_Start_Global_Replanner t) 
(Log_new_obstacle t) 


(Loiter_and_Start_Local_Replanner t) 


(t nil))) 
rote naan tenn nnn nena nnn ne n= NAVIGATOR QUERIES ------------------------------- 
(defmethod query ((navigator navigatorclass) question) 
(update-position navigator) 
(case question 
(Is_recovery_point_obtainable 
(distance (last-position navigator) (target-way-point navigator))) 


(Initialization_complete 
(cond ((and (equalp (power-status (imu auv)) on) 

(equalp (power-status (depth-cell auv)) “on) 
(equalp (power-status (gps auv)) ‘on) 
(equalp (power-status (uhf-receiver auv)) ‘on) 
(equalp (power-status (auto-pilot auv)) ‘on)) 

(write-line “NAV -->OOD: yes” ) t) 

(t (write-line “NAV -->OOD: no’”’) nil))) 


(Initialization_aborted 
(initialize (imu auv)) 
(initialize (depth-cell auv)) 
(initialize (gps auv)) 
(initialize (uhf-receiver auv)) 
(initialize (auto-pilot auv)) 
(write-line “NAV -->OQOD: no’) nil) 
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(Task_location_reached 
(cond ((target-point-reached navigator) 
(write-line “NAV -->OQOD: yes’’) t) 
(t (write-line “NAV -->OOD: no’) nil))) 


(At_recovery_point 
(cond ((target-point-reached navigator) 
(write-line “NAV -->OQOD: yes’’) t) 
(t (write-line “NAV -->OQOD: no’”’) nil))) 


(Waypoint_reached 
(cond ((target-point-reached navigator) 
(write-line “NAV -->OQOD: yes’) t) 
(t (write-line “NAV -->OOD: no’) nil))) 


(Waypoint_process_OK 
(check-setpoints-modes navigator) 
(write-line “NAV -->OOD: yes’ ) t) 


(Got_next_waypoint 
(cond ((and (equalp (first (target-way-point navigator)) “R) 
(null (way-point-list navigator))) 
(write-line “NAV -->OOD: no’’) nil) 
(t (write-line “NAV -->OQOD: yes’) t))) 


(GPS_fix_needed 
(cond((< 60 (- (current-time auv-clock) (last-gps-time navigator))) 
(write-line “NAV -->OOD: yes” ) t) 
(t(write-line “NAV -->OQOD: no’’) nil))) 


(GPS_fix_obtained 
(cond ((equalp (gps-obtained-flag) “yes) 
(write-line “NAV -->OOD12: yes” ) t) 
(t(write-line “NAV -->OOD13: no”) nil))) 


(Abort_GPS_fix 
(cond ((and (equalp (power-status gps auv) “on) 
(equalp (power-status uhf-receiver auv) “on)) 
(write-line “NAV -->OQOD: no’’) nil) 
(t(write-line “NAV -->OOD: yes” ) t))) 


(Area_clear_of_uncharted_obstacles 
(write-line “NAV -->OOD: yes” ) t) 
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(New_obstacle_logged 
(write-line “NAV -->OQOD: yes” ) t) 


(Log_system_failure 
(write-line “NAV -->OOD: no’) nil) 


(Setpoints_and_modes_sent 
(cond ((check-setpoints-modes navigator) 
(write-line “NAV -->OOD: yes” ) t) 
(t(write-line “NAV -->OOD: no’”’) nil))) 


(Setpoints_and_modes_system_OK 
(cond ((check-setpoints-modes navigator) 
(write-line “NAV -->OOD: yes” ) t) 
(t(write-line “NAV -->OQOD: no’’) nil))) 


(t nil) )) 


C. SUB.CL 


(defclass sub-class (rigid-body) 

((gps 
:initform (make-instance ‘gps-class) 
"accessor gps) 

(uhf-receiver 
‘initform (make-instance ‘uhf-receiver-class) 
-accessor uhf-receiver) 

(imu 
:initform (make-instance ‘imu-class) 
saccessor imu) 

(depth-cell 
‘initform (make-instance ‘depth-cell-class) 
accessor depth-cell) 

(speed-sensor 
:initform (make-instance 'speed-sensor-class) 
“accessor speed-sensor) 

(compass 
:initform (make-instance ‘compass-class) 
saccessor compass) 

(auto-pilot 
:initform (make-instance ‘auto-pilot-class) 
accessor auto-pilot) 

(ins 
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‘initform (make-instance 'insprocessclass) 


saccessor ins) 


(camera 


‘initform (make-instance 'strobe-camera) 


-accessor camera) 


(sigma 


‘initform 2 
‘accessor sigma) 


(kappa 


— tinitform 0 
saccessor kappa) 


(a 


"acceSSOr a) 


(b 


saccessor b) 


(c 


‘acCeSSOr C) 
(node-list 


‘initform '((0 0 0 1) 


(-5 -2-11);1 
(-5 2-11) ;2 
(5211) 33 
(-5 -211) 34 
(-4011) ;5 
(2011) ;6 
(4031) ;7 
(-40-31) 38 
(-20-11) 39 
(-40-11) ;10 
(52-11) ;11 
(5211) 312 
(10001) 313 
(10101) ;14 
(10-101) 315 
(5-2-11) 316 
(5-211) 317 
(1-401) ;18 
(3-201) ;19 
(1-201) 320 
(1201) 321 
(3201) 322 
(140 1))) 323 


0 
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(polygon-list 

‘initform '((1 23 4) ;backview 
(2 11123) ;rt side 
(8910) top fin 
(567)  ;bottom fin 
(11 12 14) srt cone 
(11 14 15 16);top cone 
(2 11161) ;top 
(18 19 20) ;it fin 
(21 22 23) srt fin 
(1 417 16) ;lt side 
(15 1617) ;lt cone 
(43 1217) ;bottom 
(12 14 15 17)))));bottom cone 


(defmethod initialize ((auv sub-class)) 
(move (camera auv) 0 (- (/ pi 2)) 0 -100 -100 -100) 
(set-sigma auv) 
(setf (transformed-node-list auv) (node-list auv)) 
(setf (velocity-growth-rate auv) (update-velocity-growth-rate auv)) 
(setf (posture-rate auv) (earth-velocity auv)) 
(take-picture (camera auv) auv) 
(setf (time-stamp auv) (current-time auv-clock))) 


‘set linearization constants 
(defmethod set-sigma ((auv sub-class)) 
(let* (( curvature (/ 1 (sigma auv)))) 
(setf (a auv) (* 3 curvature)) 
(setf (b auv) (* 3 (power 2 curvature))) 
(setf (c auv) (power 3 curvature)))) 


(defmethod update-rigid-body ((auv sub-class)) ;Euler integration. 
(let* ((delta-t (get-delta-t auv))) 

(update-posture auv delta-t) 

(setf (H-matrix auv) (homogeneous-transform (sixth (posture auv)) 
(fifth (posture auv)) (fourth (posture auv)) (first (posture auv)) 
(second (posture auv)) (third (posture auv)))) 

(transform-node-list auv) 

(update-velocity auv delta-t) 

(update-velocity-growth-rate-1 auv delta-t))) 
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(defmethod update-velocity-growth-rate-1 ((auv sub-class) delta-t) 
(setf (velocity-growth-rate auv) 


(list (update-linear-velocity auv delta-t) 0 0 
(update-p auv) (update-q auv)(update-r auv delta-t)))) 


(defmethod update-r ((auv sub-class) delta-t) 
(let* ((A (dk-ds auv))) 


(setf (kappa auv) (+ (kappa auv) (* A (first (velocity auv)) delta-t))) 
(* A (square (first (velocity auv))) delta-t))) 


(defmethod dk-ds ((auv sub-class)) 
(with-slots (kappa a bc) auv 
(neg (+ (* a kappa) 


(* b (fee (- (angle-trans (sixth (posture auv))) 


(angle-trans (third (Commanded-track (auto-pilot auv))))))) 
(* c (delta-d auv)))))) 


(defmethod delta-d ((auv sub-class)) 
(let* ((x (first (posture auv))) 
(y (second (posture auv))) 


(xg (first (commanded-track (auto-pilot auv)))) 
(yg (second (commanded-track (auto-pilot auv)))) 


(theta-desired (fee (third (Commanded-track (auto-pilot auv)))))) 
(+ (neg (* (- x xg) (sin theta-desired))) (* (- y yg) (cos theta-desired))))) 


(defmethod update-q ((auv sub-class)) 
(let* 


((depth (third (posture auv))) 
(depth-commanded (slot-value (auto-pilot auv) ‘commanded-depth)) 
(k-theta (/ 1 50)) 


(theta-commanded (* (- depth-commanded depth) k-theta)) 
(theta (fifth (posture auv))) 
(k-q (/ 1 10))) 


(* (- theta-commanded theta) k-q))) 


(defmethod update-p ((auv sub-class)) 
(* (fourth (posture auv)) (/ 1 2))) 
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(defmethod update-linear-velocity ((auv sub-class) delta-t) 
(let* 
((u (first (velocity auv))) 
(u-commanded (slot-value (auto-pilot auv) 'commanded-speed)) 
(k-r (/ 1 1))) 
(* (* (- u-commanded uw) k-r) delta-t))) 


(defmethod update-auv-position ((auv sub-class)) 
(dotimes (i (* (timetick auv-clock) 10) (take-picture (camera auv) auv)) 
(update-rigid-body auv) 
(update-ins (ins auv)))) 


ELLE EET CEE ELELE EE EEE REE EE ae ee eee teen nn ee ae eee eee 


(defclass blackbox (rigid-body) 
((power-status 
‘initform ‘off 
:accessor power-status)) 


) 


(defmethod initialize ((box blackbox)) 
(setf (power-status box) ‘on)) 


PLECEE ELLE TELLER EEE ELT TELE ETE Cee ee ee ee eee eee 


(defclass gps-class (blackbox) 

((satellites-in-view 

‘initform nil 

-accessor satellites-in-view) 
(differential-correction 

‘initform '(0 0 0) 

-accessor differential-correction) 
(updated-position 

‘initform '(1 2 3) 

:accessor updated-position) 
(position-fix-obtained 

‘initform ‘no 

accessor position-fix-obtained)) 


) 


(defmethod get-gps-fix ((gps gps-class)) 
(put-satellites-in-view gps) 
(setf (updated-position gps) 
(apply-differential-correction gps (firstn 3 (posture auv))))) 
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(defmethod apply-differential-correction ((gps gps-class) gps-position) 
(with-slots (differential-correction position-fix-obtained) gps 
(setf differential-correction (get-differential-correction (uhf-receiver auv))) 
(setf position-fix-obtained ‘yes) 
(vector-add gps-position differential-correction))) 


(defmethod put-satellites-in-view ((gps gps-class)) 
(with-slots (satellites-in-view) gps 
(setf satellites-in-view nil) 
(do* 
((generator 0 (random 10000)) 
(in-view  satellites-in-view (if (> generator 9000) (cons 1 in-view) in-view))) 
((> (length in-view) 2) t)))) , 


o of 2k ok of ok 2k 2k 2k 2 of of 2 2k 2K 2k 2k 2 ok 3 2 2 2 2 OK OK OK OK KK OK KK KK OK KK OK KK KK KK OK OK OK OK KK KK KKK KK 
(defclass uhf-receiver-class (blackbox) 


((differential-correction 
sinitform ‘(0 0 0) 
-accessor differential-correction)) 


) 


(defmethod get-differential-correction ((uhf-receiver uhf-receiver-class)) 
(update-differential-correction uhf-receiver) 
(differential-correction uhf-receiver)) 


(defmethod update-differential-correction ((uhf-receiver uhf-receiver-class)) 


t) 


o ke 2fe 3k of 2 ie 2 fe 2h ok fe 2 fe afc 24 ic 2s 2 2 2 3k 2k 9 2 2 ok ok 3 of 2k 2k 2 2k 2k 2h 2 oe 2 2k 2 oe 2k 2 2 2 2k a 2 kg 2k oo OK 6 OB OK 
? 


(defclass imu-class (blackbox) ()) 


(defmethod update-imu ((imu imu-class)) 
(let* 
((phi (fourth (posture auv))) 
(theta (fifth (posture auv))) 
(u (first (velocity auv))) 
(v (second (velocity auv))) 
(w (third (velocity auv))) 
(p (fourth (velocity auv))) 
(q (fifth (velocity auv))) 
(r (sixth (velocity auv))) 
(u-dot (first (velocity-growth-rate auv))) 
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(v-dot (second (velocity-growth-rate auv))) 
(w-dot (third (velocity-growth-rate auv)))) 
(list 
(+ u-dot (+ (* -1 vr) (+ (* wq) (* *gravity* (sin theta))))) 
(+ v-dot (+ (* -1 wp) (- (* ur) (* *gravity* (cos theta) (sin phi))))) 
(+ w-dot (+ (* -1 uq) (- (* v p) (* *gravity* (cos theta) (cos phi))))) 
(fourth (velocity-growth-rate auv)) 
(fifth (velocity-growth-rate auv)) 
(sixth (velocity-growth-rate auv))))) 


(defmethod initialize ((imu imu-class)) 
(setf (power-status imu) 'on)) 
: (setf (last-access-time imu) (current-time auv-clock))) 


wo ake a 3K 2 oe ae oo Ko oe oe a KK RO ke aK ak ok 9k 2k 9k 2 ok KK 2 ok ok ak ok 2 ok 2 aK ae a oR OK KK KK KK 
? 


(defclass depth-cell-class (blackbox) 


((pressure 
‘initform 0 ;pounds/sqr inch 
‘accessor pressure)) 


) 


(defmethod get-depth ((depth-cell depth-cell-class)) 
(setf depth (* (pressure depth-cell) (/ 32.0 14.7)))) 


oo 2k ok 2k ok ok 2 2k 2 2k 2 2 2 ok 2 2K 2k 2K eo Ko ok 2 ok 2K ok ok eo ok oo KB KK OK OK 2 OK OK eK OK OK KK KKK 
? 


(defclass speed-sensor-class (blackbox)()) 


(defmethod get-speed ((speed-sensor speed-sensor-class)) 
(first (velocity auv))) 


o 2 of ok ok of ofc ok 2 ke 2k oe 2 24 2k oe 2h 3 2k 2 2 2k 2k 2 2K 2 ok 2K 2 ok oo ok 9 ok oh OK Hh KK OK EK KKK KK RK KK 
? 


(defclass compass-class (blackbox)()) 


(defmethod get-heading ((compass compass-class)) 
(sixth (posture auv))) 


RXRELELER ES ELL LEL SA ERECT ERC EAE EEL LEE ERE EER EE RESTS 


(defclass auto-pilot-class (blackbox) 
((commanded-track 
‘initform ‘(0 0 0) 
saccessor commanded-track) 


174 





(commanded-speed 

:initform 0 

:accessor commanded-speed) 
(commanded-depth 

:initform 0 

:accessor commanded-depth) 
(commanded-dive-angle 

‘initform 0 

saccessor commanded-dive-angle))) 


(defmethod update-helm-commands ((auto-pilot auto-pilot-class) commands) 
(setf (commanded-track auto-pilot) (first commands)) 
(setf (commanded-speed auto-pilot) (second commands)) 
(setf (commanded-depth auto-pilot) (third commands)) 
(setf (Commanded-dive-angle auto-pilot) (fourth commands))) 


;(defmethod auto-pilot-position ((auto-pilot auto-pilot-class)) 
; (firstn 3 (posture auv))) 


ooh 2k 2 2k 2k 2 2k 2k 2k 2 oe ok 2k 2 ok Kok 2 Ok ok ok ok 2K OK 2 ok ok ok 2k ok 2 ok Ke 2 2 OK 2 2 2 2 3 2 2k 2 9 2 2s oe 2 OK 2K OK 2K OK OK OK Ok 
> 


D. INS.CL 


(defclass insprocessclass () 

((estimated-posture 

‘initform ‘(0 0 Q) ;xe ye ze 

accessor estimated-posture) 
(estimated-attitude 

‘initform ‘(0 0 0) ;phi theta psi 

saccessor estimated-attitude) 
(estimated-linear-velocity 

‘initform ‘(0 0 0) ;u vw 

accessor estimated-linear-velocity) 
(estimated-angular-rates 

‘initform ‘(0 0 0) ;pqr 

-accessor estimated-angular-rates) 
(last-gps-time 

‘initform 0 

accessor last-gps-time) 
(error-vector 

sinitform ‘(0 0 0) 
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‘accessor error-vector) 
(k-one 
‘initform ‘(0.0 0.0) 
saccessor k-one) 
(k-two 
‘initform 0.0 
saccessor k-two) 
(k-three 
‘initform '((0.0 0.0 0.0) 
(0.0 0.0 0.0) 
(0.0 0.0 0.0)) 
saccessor k-three) 
(k-four 
‘initform ‘(1.0 1.0 1.0) 
saccessor k-four))) 
(defmethod ins-position ((ins insprocessclass)) 
(firstn 3 (estimated-posture ins))) 


(defmethod correct-position((ins insprocessclass) position time) 
(with-slots (estimated-posture error-vector last-gps-time k-four) ins 
(let* ((delta-x (- (first position) (first estimated-posture))) 
(delta-y (- (second position) (second estimated-posture))) 
(delta-z (- (third position) (third estimated-posture))) 
(delta-t (- time last-gps-time))) 
(setf estimated-posture position) 
. (setf error-vector (vector-add error-vector (list 
(* (first k-four) (/ delta-x delta-t)) 
(* (second k-four) (/ delta-y delta-t)) 
(* (third k-four) (/ delta-z delta-t)))))) 
(setf last-gps-time time))) 


(defmethod update-ins ((ins insprocessclass)) 


(with-slots (estimated-attitude estimated-angular-rates estimated-posture 


estimated-linear-velocity) ins 

(let* (amu-output (update-imu (imu auv))) 
(speed (get-speed (speed-sensor auv))) 
(heading (get-heading (compass auv)))) 
(setf estimated-angular-rates (rate-filter ins imu-output heading)) 
(setf estimated-attitude (integrate-euler-rates ins)) 
(setf estimated-linear-velocity (estimated-earth-velocity 

ins (firstn 3 imu-output) speed)) 

(setf estimated-posture (integrate-velocities ins))))) 
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(defmethod rate-filter ((ins insprocessclass)imu-output heading) 
(with-slots (estimated-attitude k-one k-two) ins 
(let* ((theta-a (asin (/ (first imu-output) *gravity*))) 
(phi-a (neg (asin (/ (second imu-output) 
(* *gravity* (cos (second estimated-attitude))))))) 
(phi-filter (* (first k-one) (- phi-a (first estimated-attitude)))) 
(theta-filter (* (second k-one) (- theta-a (second estimated-attitude)))) 
(psi-filter (* k-two (- heading (third estimated-attitude)))) 
(filter-correction (list phi-filter theta-filter psi-filter)) 
(unfiltered-euler-rates (post-multiply (body-rate-to-euler-rate-matrix 
(third estimated-attitude) 
(second estimated-attitude) 
(first estimated-attitude)) 
(list 
(fourth imu-output) 
(fifth imu-output) 
(sixth imu-output))))) 
(vector-add unfiltered-euler-rates filter-correction)))) 


(defmethod integrate-euler-rates ((ins insprocessclass)) 
(with-slots (estimated-attitude estimated-angular-rates) ins 
(vector-add estimated-attitude 
(scalar-multiply (get-delta-t auv) estimated-angular-rates)))) 


(defmethod estimated-earth-velocity ((ins insprocessclass) specific-forces speed) 
(with-slots (estimated-attitude estimated-linear-velocity error-vector k-three) ins 
(let* ((x-linear-accel (- (first specific-forces) 
(* *gravity* 
(sin (second estimated-attitude))))) 
(y-linear-accel (+ (second specific-forces) 
(* *gravity* 
(sin (first estimated-attitude)) 
(cos (second estimated-attitude))))) 
(z-linear-accel (+ (third specific-forces) 
(* *gravity* 
(cos (first estimated-attitude)) 
(cos (second estimated-attitude))))) 
(r-matrix (rotation-matrix (third estimated-attitude) 
(second estimated-attitude) 
(first estimated-attitude))) 
(earth-accels (post-multiply r-matrix 
(list x-linear-accel 
y-linear-accel 
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z-linear-accel))) 
(speed-vector (post-multiply r-matrix (list speed 0 0))) 
(filter-correction (post-multiply k-three 
(vector-add (scalar-multiply -1 estimated-linear-velocity) 
(vector-add speed-vector error-vector))))) 

(vector-add estimated-linear-velocity 

(scalar-multiply (get-delta-t auv) 

(vector-add filter-correction 
earth-accels)))))) 


(defmethod integrate-velocities ((ins insprocessclass)) 
(with-slots (estimated-posture estimated-linear-velocity) ins 


(vector-add estimated-posture 
(scalar-multiply (get-delta-t auv) estimated-linear-velocity)))) 


;(ioad ‘replanner.cl) 
(load ‘world.cl) 
(defclass replannerclass()() ) 


(defmethod plan-route ((replanner replannerclass) obstacles beginning end) 
(list beginning end)) 


E. ROBOT-KINEMATICS.CL 


File: robot-kinematics.cl Franz Common LISP 


; by Dr. McGhee for CS4314 


of ok ok of 2h 2k ak ok oe 2h 2 ok 2 2 2 2 2 2 2 2 2 oo 2 a fe ok Ko of 2 3 ke 2 2 2K HK OK he OK 2h a 2 2s he A Oe ee OK OK EK OK OK KK 
’ 


(defun transpose (matrix) ;A matrix is a list of row vectors. 
(cond ((null (cdr matrix)) (mapcar ‘list (car matrix))) 
(t (mapcar ‘cons (car matrix) (transpose (cdr matrix)))))) 


(defun dot-product (vector-1 vector-2) ;A vector is a list of numerical atoms. 
(apply '+ (mapcar '* vector-1 vector-2))) 


(defun vector-magnitude (vector) (sqrt (dot-product vector vector))) 
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(defun post-multiply (matrix vector) 
(cond ((null (rest matrix)) (list (dot-product (first matrix) vector))) 
(t (cons (dot-product (first matrix) vector) 
(post-multiply (rest matrix) vector))))) 


(defun pre-multiply (vector matrix) 
(post-multiply (transpose matrix) vector)) 


(defun matrix-multiply (A B) —__;A and B are conformable matrices. 
(cond ((null (cdr A)) (list (pre-multiply (car A) B))) 
(t (cons (pre-muitiply (car A) B) (matrix-multiply (cdr A) B))))) 


(defun chain-multiply (L) -L is a list of names of conformable matrices. 
(cond ((null (cddr L)) (matrix-multiply (eval (car L)) (eval (cadr L)))) 
(t (matrix-multiply (eval (car L)) (chain-multiply (cdr L)))))) 


(defun cycle-left (matrix) (mapcar 'row-cycle-left matrix)) 
(defun row-cycle-left (row) (append (cdr row) (list (car row)))) 
(defun cycle-up (matrix) (append (cdr matrix) (list (car matrix)))) 


(defun unit-vector (one-column length) ;Column count starts at 1. 
(do ((n length (1- n)) 
(vector nil (cons (cond ((= one-column n) 1) (t 0)) vector))) 
((zerop n) vector))) 


(defun unit-matrix (size) 
(do ((row-number size (1- row-number)) 
(I nil (cons (unit-vector row-number size) I))) 
((zerop row-number) I))) 


(defun concat-matrix (A B) ;A and B are matrices with equal number of rows. 
(cond ((null A) B) 3 
(t (cons (append (car A) (car B)) (concat-matrix (cdr A) (cdr B)))))) 


(defun augment (matrix) 
(concat-matrix matrix (unit-matrix (length matrix)))) 


(defun normalize-row (row) (scalar-multiply (/ 1.0 (car row)) row)) 


(defun scalar-multiply (scalar vector) 
(cond ((null vector) nil) 
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(t (cons (* scalar (car vector)) 
(scalar-multiply scalar (cdr vector)))))) 


(defun solve-first-column (matrix) ‘Reduces first column to (1 0... 0). 
(do* ((remaining-row-list matrix (rest remaining-row-list)) 
(first-row (normalize-row (first matrix))) 
(answer (list first-row) | 
(cons (vector-add (first remaining-row-list) 
(scalar-multiply (- (caar remaining-row-list)) 
first-row)) 
answer))) 

((null (rest remaining-row-list)) (reverse answer)))) 

(defun vector-add (vector-1 vector-2) (mapcar '+ vector-1 vector-2)) 


(defun vector-subtract (vector-1 vector-2) (mapcar '- vector-1 vector-2)) 


(defun first-square (matrix) ;Returns leftmost square matrix from argument. 
(do ((size (length matrix)) 
(remainder matrix (rest remainder)) 
(answer nil (cons (firstn size (first remainder)) answer))) 
((null remainder) (reverse answer)))) 


(defun firstn (n list) 
(cond ((zerop n) nil) 
(t (cons (first list) (firstn (1- n) (rest list)))))) 


(defun max-car-firstn (n list) 
(append (max-car-first (firstn n list)) (nthcdr n list))) 


(defun matrix-inverse (M) 
(do ((M1 (max-car-first (augment M)) 
(cond ((null M1) nil) ;Abort for singular matrix. 
(t (max-car-firstn n (cycle-left (cycle-up M1)))))) 
(n (1- (length M)) (1- n))) 
((or (minusp n) (null M1)) (cond ((null M1) nil) (t (first-square M1)))) 
(setq M1 (cond ((zerop (caar M1)) nil) (t (solve-first-column M1)))))) 


(defun max-car-first (L) ;L isa list of lists. This function finds list with 
(cond ((null (cdr L)) L) ;largest car and moves it to head of list of lists. 
(t (if (> (abs (caar L)) (abs (caar (max-car-first (cdr L))))) L 
(append (max-car-first (cdr L)) (list (car L))))))) 
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(defun dh-matrix (cosrotate sinrotate costwist sintwist length translate) 
(list (list cosrotate (- (* costwist sinrotate)) 
(* sintwist sinrotate) (* length cosrotate)) 
(list sinrotate (* costwist cosrotate) 
(- (* sintwist cosrotate)) (* length sinrotate)) 
(list 0. sintwist costwist translate) (list 0. 0. 0. 1.))) 


(defun homogeneous-transform (azimuth elevation roll x y z) 
(let ((spsi (sin azimuth)) (cpsi (cos azimuth)) (sth (sin elevation)) 
(cth (cos elevation)) (sphi (sin roll)) (cphi (cos roll))) 
(list (list (* cpsi cth) (- (* cpsi sth sphi) (* spsi cphi)) 
(+ (* cpsi sth cphi) (* spsi sphi)) x) 
(list (* spsi cth) (+ (* cpsi cphi) (* spsi sth sphi)) 
(- (* spsi sth cphi) (* cpsi sphi)) y) 
(list (- sth) (* cth sphi) (* cth cphi) z) 
(list 0. 0. 0. 1.)))) 


(defun inverse-H (H) [H is a 4x4 homogeneous transformation matrix. 
(let* ((minus-P (list (- (fourth (first H))) 
(- (fourth (second H))) 
(- (fourth (third H))))) 
(inverse-R (transpose (first-square (reverse (rest (reverse H)))))) 
(inverse-P (post-multiply inverse-R minus-P))) 
(append (concat-matrix inverse-R (transpose (list inverse-P))) 


(list dist 0 0 0 1))))) 


(defun rotation-matrix (azimuth elevation roll) 
(let ((spsi (sin azimuth)) (cpsi (cos azimuth)) (sth (sin elevation)) 
(cth (cos elevation)) (sphi (sin roll)) (cphi (cos roll))) 
(list (list (* cpsi cth) (- (* cpsi sth sphi) (* spsi cphi)) 
(+ (* cpsi sth cphi) (* spsi sphi))) 
(list (* spsi cth) (+ (* cpsi cphi) (* spsi sth sphi)) 
(- (* spsi sth cphi) (* cpsi sphi))) 
(list (- sth) (* cth sphi) (* cth cphi))))) 


(defun body-rate-to-euler-rate-matrix (azimuth elevation roll) 
(let ((sth (sin elevation)) (cth (cos elevation)) (tth (tan elevation)) 
(sphi (sin roll)) (cphi (cos roll))) 
(list (list 1 (* tth sphi) (* tth cphi)) 
(list 0 cphi (- sphi)) 
(list 0 (/ sphi cth) (/ cphi cth))))) 
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(setf T'((1 2) (3 4))) 
(setf M '((1 2 3) (45 6) (7 8 9))) 
(setf x ‘(1 2 3)) 


F. EULER-ANGLE-RIGID-BODY.CL 


File: euler-angle-rigid-body.cl Franz Common LISP 


; by Dr. McGhee for CS4314 


woke ake ke oe ok oe oo ak ok a ok ok ake ok ok ok 3 i a oo a oo aK oR ao aK a a KK KK KK KK KK ERE KK RK KK KK 
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(defclass rigid-body 
() 
((posture ‘The vector (xe ye ze phi theta psi). 
‘initform '(0 0 0 0 0 3.14) 
‘initarg :posture 
accessor posture) 
(posture-rate ;The vector (xe-dot ye-dot ze-dot phi-dot theta-dot psi-dot). 
‘initarg :posture-rate 
accessor posture-rate) 
(velocity ‘The six-vector (u v w p qr) in body coordinates. 
‘initform ‘(0 0 0 0 0 0) 
‘initarg :velocity 
accessor velocity) 
(velocity-growth-rate ;The vector (u-dot v-dot w-dot p-dot q-dot r-dot). 
accessor velocity-growth-rate) 
(forces-and-torques ;The vector (Fx Fy Fz L MN) in body coordinates. 
-initform (list 0 0 (- *gravity*) 0 0 0) 
-accessor forces-and-torques) 
(moments-of-inertia ;The vector (Ix ly Iz) in principal axis coordinates. 
‘initform ‘(1 1 1) 
-initarg :moments-of-inertia 
‘accessor moments-of-inertia) 
(mass 
‘initform 1 
‘initarg :mass 
‘accessor mass) 
(node-list ;(x y z 1) in body coord for each node. Starts with (0 0 0 1). 
‘initform '((000 1) (400 1) 200 1) (400 1) (-5 0 -2 1) 
(-6 -1.5 -2 1) (-6 1.5 -2 1) (-2 6 -2 1) (-2 -6 -2 1) 
(-2 00 1)) ;Defines a simple "airplane" as default rigid body. 
‘initarg :node-list 
-accessor node-list) 
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(polygon-list 

‘initform '((1 3 45 43) (46) (7 28 9)) 
‘initarg :polygon-list 

accessor polygon-list) 
(transformed-node-list ;(x y z 1) in earth coord for each node in node-list. 
saccessor transformed-node-list) 
(H-matrix 

‘initform (unit-matrix 4) 

saccessor H-matrix) 

(time-stamp 

:accessor time-stamp))) 


(defmethod initialize ((body rigid-body)) 
(setf (transformed-node-list body) (node-list body)) 
(setf (velocity-growth-rate body) (update-velocity-growth-rate body)) 
(setf (posture-rate body) (earth-velocity body)) 
(setf (time-stamp body) (get-internal-real-time))) 


(defmethod move-body ((body rigid-body) azimuth elevation roll x y z) 
(setf (posture body) (list x y z roll elevation azimuth)) 
(setf (H-matrix body) 
(homogeneous-transform azimuth elevation roll x y z)) 
(transform-node-list body)) 


(defmethod get-delta-t ((body rigid-body)) 0.1) 


(defmethod update-rigid-body ((body rigid-body)) —;Euler integration. 
(let* ((delta-t (get-delta-t body))) 

(update-posture body delta-t) 

(setf (H-matrix body) (homogeneous-transform (sixth (posture body)) 
(fifth (posture body)) (fourth (posture body)) (first (posture body)) 
(second (posture body)) (third (posture body)))) 

(transform-node-list body) 

(update-velocity body delta-t) 

(update-velocity-growth-rate body))) 


(defmethod update-velocity-growth-rate ((body rigid-body)) 
(setf (velocity-growth-rate body) ;Assumes principal axis coordinates with 


(multiple-value-bind Origin at center of gravity of body. 
(Fx Fy Fz LMN uvwpgqr Ix ly Iz) ;Declares local variables. 
(values-list ; Values assigned. 

(append 


(forces-and-torques body) (velocity body) (moments-of-inertia body))) 
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(list (+ (* vr) (* -1 w q) (/ Fx (mass body)) 
(* *gravity* (first (third (H-matrix body))))) 
(+ (* w p) (*-1 ur) (/ Fy (mass body)) 
(* *gravity* (second (third (H-matrix body))))) 
(+ (* ug) (*-1 v p) (/ Fz (mass body)) 
(* *pravity* (third (third (H-matrix body))))) 
(/ (+ (* ( Ty Iz) qr) L) Ix) 
(/ (+ (* ( Iz Ix)r p) M) ly) 
(/ (+ (* (- Ix Iy) pq) N) Iz))))) ; Value returned. 


(defmethod update-velocity ((body rigid-body) delta-t) ;Euler integration. 
(setf (velocity body) 
(vector-add (velocity body) 
(scalar-multiply delta-t (velocity-growth-rate body))))) 


(defmethod update-posture ((body rigid-body) delta-t) ;Euler integration. 
(setf (posture-rate body) (earth-velocity body)) 
(setf (posture body) 
(vector-add (posture body) (scalar-multiply delta-t (posture-rate body))))) 


(defmethod transform-node-list ((body rigid-body)) 
(setf (transformed-node-list body) 
(mapcar #'(lambda (node-location) 
(post-multiply (H-matrix body) node-location)) 
(node-list body)))) 


(defconstant *gravity* 32.2185) 


(defmethod earth-velocity ((body rigid-body)) 
(let* ((translational-velocity (list(first (velocity body)) 
(second (velocity body)) 
(third (velocity body)))) 
(rotational-velocity (list(fourth (velocity body)) 
(fifth (velocity body)) 
(sixth (velocity body))))) 
(append (post-multiply (rotation-matrix (sixth (posture body)) 
(fifth (posture body)) 
(fourth (posture body))) 
translational-velocity) 
(post-multiply (body-rate-to-euler-rate-matrix (sixth (posture body)) 
(fifth (posture body)) 
| (fourth (posture body))) 
rotational-velocity)))) ) 
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(defun test-rigid-body ( 
(setf airplane-1 (make-instance 'rigid-body)) 
(initialize airplane- 1) 
(setf camera-1 (make-instance ‘strobe-camera)) 
(move camera-1 0 (- (/ pi 2)) 0 0 0 -30) 
(take-picture camera-1 airplane-1) 
(dotimes (i 20 'done) (update-rigid-body airplane-1)) 
(take-picture camera-1 airplane-1)) 


G. WIND-TRI.CL 
(defconstant two-pi 6.283185307179586) ;pi multiplied by two 
(defconstant pi-two 1.5707963267948966) ;pi divided by two 


EEA LE RES Ree REE ACLOCK CLASS BERR KKK KK KG RK RK RK RK RE KEK 


(defclass clock-class () 


((time-count 
-initform 0 
‘accessor time-count) 


(timetick 
‘initform 1 
‘accessor timetick))) 


(defmethod current-time ((clock clock-class)) 
(time-count clock)) 


(defmethod increment-time ((clock clock-class)) 
(with-slots (time-count timetick) clock 


(setf time-count (+ time-count timetick)))) 


oo KKEKKKKKKKK KKK KKK KREKKKKKAKKKKKKKKKKEKKKKKKKAKKKKARKKKAKKKEKKK KKK KKK KKK 
>? 
(defun radian-true-course (from-point to-point) 

(positive-radians (atan (- (second to-point) (second from-point)) 


(- (first to-point) (first from-point))))) 


(defun positive-radians (radians) 
(if (< radians QO) (+ radians two-pi) radians)); 
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(defun calculate-heading (speed course current) 
(cond ((zerop speed) course) 
(t (let* ((A (- course (second current))) 
(b (first current)) 
(a speed) 
(B (asin (/ (* b (sin A)) a)))) 
(+ course B))))) 


(defun square (x) (* x x)) 
(defun deg-to-rad (deg) (* deg (/ pi 180))) 
(defun rad-to-deg (rad) (* rad (/ 180 pi))) 


(defun distance (from-point to-point) 
(let*((x-delta (- (first to-point) 
(first from-point))) 
(y-delta (- (second to-point) 
(second from-point)))) 
(sqrt (+ (square x-delta) (square y-delta))))) 


(defun find-vector (from-point to-point) 
(list (distance from-point to-point) 
(radian-true-course from-point to-point))) 


(defun firstj (n list) ;improved version 
(cond ((or(zerop n)(null list)) nil) 
(t (cons (first list) (firstj (1- n) (rest list)))))) 


(defun power (exponent base) 
(do* ((i exponent (1- 1)) 
(result base (* result base))) 
((=1 1) result))) 


(defun neg (number) 
(* -] number)) 


(defun angle-trans (heading) 
(if (> heading pi) (- heading two-pi) heading)) 


snormalizes alpha to between pi and -pi 


(defun fee (alpha) 
(if (> alpha pi) (fee(- alpha two-pi)) 
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(if (<= alpha (neg pi)) (fee(+ alpha two-pi)) alpha))) 


H. CAMERA.CL 


- File: camera.cl Franz Common LISP 


; ** CAMERA CLASS DEFINITION ** 

; A Camera "takes a picture" of rigid-body class objects 

; and displays the image. A sequence of images may be 

; displayed by superimposing them or by first erasing the display 
; window and then creating and displaying the next image. 


; Requires: rigid-body.cl 


; by Shirley Isakari CS4314 Winter 1994 Final Project 


- Modifications & enhancements to Prof. McGhee's Strobe-Camera CLOS code 
: ok ok 3k 2k of 2k ok 2k 2k ok ok of 2k ok oe 2k 2 oe ok 2k 2K 2 2 2h 2K 2 2k 2k 2K ok 2k 2 of 2c 2 ok OK Ok oo KK ok OK OK Ok OK OK OK OK OK OK OK OK Ok OK OK OK OK OK OK Ke KK 


(require :xcw) 


(use-package :cw) ; Note that this is required for use of mouse and color. 
; This forced renaming of some original functions, 1.e. 
; move and translate. Causes some problem when compiling. 


(cw:initialize-common-windows) 


(defclass camera (rigid-body) 
((focal-length 
:accessor focal-length 
‘initform 6) 
(posture 
accessor posture ; azim elev roll x y z 
‘initform (list 0 0 0 -300 0 0)) 
(camera-window 
:accessor Camera-window 
‘initform (cw:make-window-stream :borders 5 
‘left 300 
sbottom 300 
:width 300 
height 400 
‘title "Right Arm Articulation" 
:background-color blue 
:‘foreground-color white 
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:activate-p t)) 
(H-matrix 
:initform (homogeneous-transform 0 0 0 -300 0 0)) 
(inverse-H-matrix 
saccessor inverse-H-matrix 
:initform (inverse-H (homogeneous-transform 0 0 0 -300 0 0))) 
(enlargement-factor 
:accessor enlargement-factor 
‘initform 900))) 


(defun create-camera-1 () 
(setf camera-1 (make-instance 'camera)) 
(queue-mouse camera-1)) 


(defmethod queue-mouse ((camera camera)) 
(cw:modify-window-stream-method (camera-window camera) :left-button-down 
‘after ‘'mouse-handler) 
(cw:modify-window-stream-method (camera-window camera) :middle-button-down 
:after '‘mouse-handler) 
(cw:modify-window-stream-method (camera-window camera) :right-button-down 
‘after 'mouse-handler)) 


; Note that mouse-handler requires names of instantiated objects: 
; camera-1 jack-1. Unable to modify argument list of this event-handler. 
(defun mouse-handler (wstream cw:mouse-state &optional event) 
‘(format t "In mouse-handler button: ~a~%" (mouse-button-state)) 
(cond ((eql (cw:mouse-button-state) 128) ; Left-click 
(rotate-camera camera-1 -10) 
; (format t “Mouse Event: Left-click => rotate-camera~%") 
) 
((eql (Ccw:mouse-button-state) 129) ; Left-click & CNTRL key 
(rotate-camera camera-1 10) 
; (format t "Mouse Event: CNTRL+Left-click => rotate-camera~%") 
) 
((eql (cw:mouse-button-state) 64) > Middle-click 
(zoom-camera camera-! 10) 
; (format t "Mouse Event: Middle-click => zoom-camera~%") 
) 
((eql (cw:mouse-button-state) 65) ; Middle-click & CNTRL key 
(zoom-camera camera-1 -10) 
; (format t "Mouse Event: CNTRL+Middle-click => zoom-camera~%") 
) 
((eql (cw:mouse-button-state) 32) ; Right-click 
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(tilt-camera camera-1 -10) 

; (format t "Mouse Event: Right-click => tilt-camera~%") 

) 

((eql (cw:mouse-button-state) 33) ; Right-click & CNTRL key 
(tilt-camera camera-1 10) 

- (format t "Mouse Event: CNTRL+Right-click => tilt-camera~%") 

) 

(t nil)) 

(new-picture camera-1 jack-1 jack-color)) 


: *k* Defined global color constants 3k 2k 2k 9 9k 9 of ok ok ok ok ok oe ok ok Ok ok 2k ok 2 ok 3 ok OK RE KK OK KK KOK KKK 


- To be used as the draw-color argument in take-picture and new-picture 
; functionss (and also jack-picture, jack-video, jack-movie functions) 


(defconstant *white* 0) 
(defconstant *yellow* 1) 
(defconstant *red* 2) 
(defconstant *green* 3) 
(defconstant *black* 4) 
(defconstant *cyan* 5) 
(defconstant *magenta* 6) 
(defconstant *blue* 7) 


: 2K OK Draw picture functions ie 2 246 2 2K 2k ois 2 of of 9 9 2 2 2 2 2 Ok 2 2 fe i 2 2 Oe Oe 2 Oe oe oe ie 2 2 oe oe Ok ok ok KK 


(defmethod take-picture ((camera camera) (body rigid-body) draw-color) 
(let ((camera-space-node-list (mapcar #'(lambda (node-location) 
(post-multiply (inverse-H-matrix camera) node-location)) 
(transformed-node-list body)))) 
(dolist (polygon (polygon-list body)) 
(clip-and-draw-polygon camera polygon camera-space-node-list draw-color)))) 


(defmethod erase-camera-window ((camera camera)) 
(cw:clear (camera-window camera))) 


(defmethod new-picture ((camera camera) (body rigid-body) draw-color) 
(erase-camera-window camera) 
(take-picture camera body draw-color)) 


(defmethod clip-and-draw-polygon 
((camera camera) polygon node-coord-list draw-color) 
(do* ((initial-point (nth (first polygon) node-coord-list)) 
(from-point initial-point to-point) 
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(remaining-nodes (rest polygon) (rest remaining-nodes)) 
(to-point (nth (first remaining-nodes) node-coord-list) 
(if (not (null (first remaining-nodes))) 
(nth (first remaining-nodes) node-coord-list)))) 
((null to-point) 
(draw-clipped-projection camera from-point initial-point draw-color)) 
(draw-clipped-projection camera from-point to-point draw-color))) 


(defmethod draw-clipped-projection ((camera camera) 
from-point to-point draw-color) 
(cond ((and (<= (first from-point) (focal-length camera)) 
(<= (first to-point) (focal-length camera))) nil) 

((<= (first from-point) (focal-length camera)) 

(draw-line-in-window camera 
(perspective-transform camera (from-clip camera from-point to-point)) 
(perspective-transform camera to-point) draw-color)) 

((<= (first to-point) (focal-length camera)) 

(draw-line-in- window camera 
(perspective-transform camera from-point) 
(perspective-transform camera (to-clip camera from-point to-point)) 

draw-color)) 

(t (draw-line-in-window camera 
(perspective-transform camera from-point) 
(perspective-transform camera to-point) draw-color)))) 


(defmethod from-clip ((camera camera) from-point to-point) 
(let ((scale-factor (/ (- (focal-length camera) (first from-point)) 
(- (first to-point) (first from-point))))) 
(list (+ (first from-point) 

(* scale-factor (- (first to-point) (first from-point)))) 

(+ (second from-point) 
(* scale-factor (- (second to-point) (second from-point)))) 

(+ (third from-point) 
(* scale-factor (- (third to-point) (third from-point)))) 1))) 


(defmethod to-clip ((camera camera) from-point to-point) 
(from-clip camera to-point from-point)) 


(defmethod draw-line-in-window ((camera camera) start end draw-color) 
(cond ((= 0 draw-color) (cw:draw-line (camera-window camera) 
(cw:make-position :x (first start) :y (second start)) 
(cw:make-position :x (first end) :y (second end)) 
=brush-width 5 :color white)) 
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((= 1 draw-color) (cw:draw-line (camera-window camera) 
(cw:make-position :x (first start) :y (second start)) 
(cw:make-position :x (first end) :y (second end)) 
-brush-width 5 :color yellow)) 

((= 2 draw-color) (cw:draw-line (camera-window camera) 
(cw:make-position :x (first start) :y (Second start)) 
(cw:make-position :x (first end) :y (second end)) 
:>brush-width 5 :color magenta)) 

((= 3 draw-color) (cw:draw-line (camera-window camera) 
(cw:make-position :x (first start) :y (second start)) 
(cw:make-position :x (first end) :y (second end)) 
-brush-width 5 :color green)) 

((= 4 draw-color) (cw:draw-line (camera-window camera) 
(cw:make-position :x (first start) :y (second start)) 
(cw:make-position :x (first end) :y (second end)) 
-brush-width 5 :color red)) 

((= 5 draw-color) (cw:draw-line (camera-window camera) 
(cw:make-position :x (first start) :y (second start)) 
(cw:make-position :x (first end) :y (second end)) 
>brush-width 5 :color cyan)) 

((= 6 draw-color) (cw:draw-line (camera-window camera) 
(cw:make-position :x (first start) :y (Second start)) 
(cw:make-position :x (first end) :y (second end)) 
‘brush-width 5 :color black)) 

((= 7 draw-color) (cw:draw-line (camera-window camera) 
(cw:make-position :x (first start) :y (second Start)) 
(cw:make-position :x (first end) :y (second end)) 
:brush-width 5 :color blue)))) 


(defmethod perspective-transform ((camera camera) point-in-camera-space) 
(let* ((enlargement-factor (enlargement-factor camera)) 

(focal-length (focal-length camera)) 

(x (first point-in-camera-space)) ;x axis is along optical axis 

(y (second point-in-camera-space)) ;y is out right side of camera 

(z (third point-in-camera-space))) ;z 1s out bottom of camera 

(list (+ (round (* enlargement-factor (/ (* focal-length y) x))) 

150) sto right in camera window 
(+ 150 (round (* enlargement-factor (/ (* focal-length (- z)) x)) 
))))) ;up in camera window 


: *k* Position Camera functions ******** KEK A KK KKK EK RK KKK EK RK KEK EK RE 


(defmethod move-camera ((camera camera) azimuth elevation roll x y z) 
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(setf (H-matrix camera) (homogeneous-transform azimuth elevation roll x y z)) 
(setf (inverse-H-matrix camera) (inverse-H (H-matrix camera))) 
(format t “camera: ~a " (posture camera)) ) 


(defmethod zoom-camera ((camera camera) zoom-amount) 
(setf (slot-value camera 'enlargement-factor) 
(+ (slot-value camera ‘enlargement-factor) zoom-amount))) 


; Rotation in x-y plane about origin 
(defmethod rotate-camera ((camera camera) angle-increment) ; in degrees 
(let* ((new-position (posture camera)) 
(radius (sqrt (+ (* (fourth new-position) (fourth new-position)) 
(* (fifth new-position) (fifth new-position))))) 
(heading (atan (fourth new-position) 
(fifth new-position))) 
(angle (deg-to-rad angle-increment)) 
(new-heading (+ heading angle))) 
(setf (first new-position) (- (first new-position) angle) 
(fourth new-position) (* radius (sin new-heading)) 
(fifth new-position) (* radius (cos new-heading)) 
(posture camera) new-position 
(H-matrix camera) (homogeneous-transform (first new-position) 
(second new-position) (third new-position) (fourth new-position) 
(fifth new-position) (sixth new-position)) 
(inverse-H-matrix camera) (inverse-H (H-matrix camera))))) 


; Vertical tilting about origin in a plane perpendicular to x-y plane 
: Max tilt (90 or -90 deg) when top or bottom view of x-y plane is achieved 
(defmethod tilt-camera ((camera camera) angle-increment) ; in degrees 
(let* ((new-position (posture camera)) 
(radius (sqrt (+ (* (fourth new-position) (fourth new-position)) 
(* (fifth new-position) (fifth new-position)) 
(* (sixth new-position) (sixth new-position))))) 
(tilt (atan (sixth new-position) 
(sqrt (+ (* fourth new-position) (fourth new-position)) 
(* (fifth new-position) (fifth new-position)))))) 
(heading (atan (fourth new-position) 
(fifth new-position))) 
(angle (deg-to-rad angle-increment)) 
(new-tilt (cond ((< (abs (+ tilt angle)) tilt-limit) (+ tilt angle)) 
(t (cond ((minusp (+ tilt angle)) (* -1 tilt-limit)) 
(t tilt-limit)))))) 
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(setf (second new-position) new-tilt 
(fourth new-position) 
(cond ((= (abs tilt) (abs new-tilt) tilt-limit) 
(fourth new-position)) 
(t (* radius (sin heading) (cos new-tilt)))) 
(fifth new-position) 
(cond ((= (abs tilt) (abs new-tilt) tilt-limit) 
(fifth new-position)) 
(t (* radius (cos heading) (cos new-tilt)))) 
(sixth new-position) 
(cond ((= (abs tilt) (abs new-tilt) tilt-limit) 
(sixth new-position)) 
(t (* radius (sin new-tilt)))) 
(posture camera) new-position 
(H-matrix camera) (homogeneous-transform (first new-position) 
(second new-position) (third new-position) (fourth new-position) 
(fifth new-position) (sixth new-position)) 
(inverse-H-matrix camera) (inverse-H (H-matrix camera))))) 


(defun deg-to-rad (angle) (* .017453292519943295 angle)) 
(defconstant tilt-limit (deg-to-rad 89.9)) 


eee Axillary TUNCHONS 4 Pt tte ee Oe ee ree een earn hem nnn ae nee ee 


(defun kill Q 
(cw:kill-common-windows)) 


(defun reset-windows () 
(kill) 
(cw:initialize-common-windows)) 


I. STROBE-CAMERA.CL 


File: strobe-camera.cl Franz Common LISP 


; ** STROBE-CAMERA CLASS DEFINITION ** 

; A Camera "takes a picture" of rigid-body class objects 
; and displays the image. A sequence of images may be 
; displayed by superimposing them. 

; Requires: rigid-body.cl 


; by Dr. McGhee for CS4314 


© REKEKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKEKKKKERKKEKKKAKKKKKKAKKAKKK AK KKK AK KK 
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(require :xcw) 
(cw:initialize-common-windows) 


(defclass strobe-camera (rigid-body) 
((focal-length 
accessor focal-length 
‘initform 6) 
(camera-window 
‘accessor Camera-window 
‘initform (cw:make-window-stream :borders 5 
‘left 500 
:bottom 500 
:width 700 
sheight 700 
‘title "AUV" 
:activate-p t)) 
(H-matrix 
-initform (homogeneous-transform 0 (/ pi 2) 0 0 0 150)) 
(inverse-H-matrix 
‘accessor inverse-H-matrix 
-initform (inverse-H (homogeneous-transform 0 (/ pi 2) 0 0 0 150))) 
(enlargement-factor 
accessor enlargement-factor 
‘initform 30))) 
(defmethod move ((camera strobe-camera) azimuth elevation roll x y z) 
(setf (H-matrix camera) (homogeneous-transform azimuth elevation roll x y z)) 
(setf (inverse-H-matrix camera) (inverse-H (H-matrix camera)))) 


(defmethod take-picture ((camera strobe-camera) (body rigid-body)) 
(let ((camera-space-node-list (mapcar #'(lambda (node-location) 
(post-multiply (inverse-H-matrix camera) node-location)) 
(transformed-node-list body)))) 
(dolist (polygon (polygon-list body)) 
(clip-and-draw-polygon camera polygon camera-space-node-list)))) 


(defmethod clip-and-draw-polygon 

((camera strobe-camera) polygon node-coord-list) 

(do* ((initial-point (nth (first polygon) node-coord-list)) 
(from-point initial-point to-point) 
(remaining-nodes (rest polygon) (rest remaining-nodes)) 
(to-point (nth (first remaining-nodes) node-coord-list) 

(if (not (null (first remaining-nodes))) 
(nth (first remaining-nodes) node-coord-list)))) 
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((null to-point) 
(draw-clipped-projection camera from-point initial-point)) 
(draw-clipped-projection camera from-point to-point))) 


(defmethod draw-clipped-projection ((camera strobe-camera) from-point to-point) 
(cond ((and (<= (first from-point) (focal-length camera)) 
(<= (first to-point) (focal-length camera))) nil) 
((<= (first from-point) (focal-length camera)) 
(draw-line-in-camera-window camera 
(perspective-transform camera (from-clip camera from-point to-point)) 
(perspective-transform camera to-point))) 
((<= (first to-point) (focal-length camera)) 
(draw-line-in-camera-window camera 
(perspective-transform camera from-point) 
(perspective-transform camera (to-clip camera from-point to-point)))) 
(t (draw-line-in-camera-window camera 
(perspective-transform camera from-point) 
(perspective-transform camera to-point))))) 


(defmethod from-clip ((camera strobe-camera) from-point to-point) 
(let ((scale-factor (/ (- (focal-length camera) (first from-point)) 
(- (first to-point) (first from-point))))) 
(list (+ (first from-point) 
(* scale-factor (- (first to-point) (first from-point)))) 
(+ (second from-point) 
(* scale-factor (- (second to-point) (second from-point)))) 
(+ (third from-point) 
(* scale-factor (- (third to-point) (third from-point)))) 1))) 


(defmethod to-clip ((camera strobe-camera) from-point to-point) 
(from-clip camera to-point from-point)) 


(defmethod draw-line-in-camera-window ((camera strobe-camera) start end) 
(cw:draw-line (camera-window camera) 
(cw:make-position :x (first start) :y (second start)) 
(cw:make-position :x (first end) :y (second end)) 
:brush-width 0)) 


(defmethod perspective-transform ((camera strobe-camera) point-in-camera-space) 
(let* ((enlargement-factor (enlargement-factor camera)) 
(focal-length (focal-length camera)) 
(x (first point-in-camera-space)) ;x axis 1s along optical axis 
(y (second point-in-camera-space)) ;y is out right side of camera 
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(z (third point-in-camera-space))) ;z is out bottom of camera 
(list (+ (round (* enlargement-factor (/ (* focal-length y) x))) 
150) sto right in camera window 
(+ 150 (round (* enlargement-factor (/ (* focal-length (- z)) x)) 
))))) ;up in camera window 


(defun test-camera () ;Produces top view of default rigid-body. 
(setf airplane-1 (make-instance 'rigid-body)) 
(initialize airplane-1) 
(setf camera-1 (make-instance 'strobe-camera)) 
(move camera-1 0 (- (/ pi 2)) 0 0 0 -30) 
(take-picture camera-1 airplane-1)) 
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APPENDIX D: Replanner Simulation Source Code (LISP) 


A. REPLANNER.CL 


(defconstant cw -1) 
(defconstant ccw 1) 
(defconstant infinity 32000) 


(defconstant invisible-tangent (list (list infinity infinity infinity) 
(list infinity infinity infinity))) 


(load ‘world.cl) 


(defclass replannerclass() 


((world 
initform ( 
‘accessor world) 


(start 
‘initform () 
saccessor Start) 


(goal 
Initform () 
saccessor goal) 


(start-tangents 
sinitform () 
saccessor Start-tangents) 


(polygon-tangents 
initform () 
:accessor polygon-tangents) 


(goal-tangents 


sinitform () 
saccessor goal-tangents) 
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(dykstra-table 
sinitform () 
saccessor dykstra-table))) 


(defmethod plan-route ((replanner replannerclass) obstacles beginning end) 
(with-slots (start goal world start-tangents polygon-tangents goal-tangents) replanner 
(setf start beginning) 
(setf goal end) 
(setf world obstacles) 
(setf start-tangents ()) 
(setf polygon-tangents ()) 
(setf goal-tangents ()) 
(find-tangents replanner world start goal) 
(find-visible-tangents replanner) 
(find-shortest-path replanner))) ;returns the path specified in vertices 


(defmethod find-tangents ((replanner replannerclass) world start goal) 
(find-start-tangents replanner) 
(find-all-polygon-tangents replanner) 
(find-goal-tangents replanner world goal)) 


(defmethod find-visible-tangents ((replanner replannerclass)) 
(with-slots (start-tangents goal-tangents) replanner 
(setf start-tangents (find-visible-first-tangents replanner start-tangents)) 
(find-visible-polygon-tangents replanner) 
(setf goal-tangents (find-visible-last-tangents replanner goal-tangents)))) 
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(load ‘dykstra-search.cl) 


(load ‘replan-functions.cl) 


B. WORLD.CL 


(setf v11 (list 1 -150 70)) 
(setf v12 (list 2 -30 10)) 
(setf v13 Cust 3 -10 50)) 
(setf v14 (list 4 -70 80)) 
(setf v21 (list 1 -30 80)) 
(setf v22 (list 2 60 10)) 
(setf v23 (list 3 90 60)) 
(setf v31 (list 1 -100 90)) 
(setf v32 (list 2 80 90)) 
(setf v33 (list 3 80 110)) 
(setf v34 (list 4 -100 110)) 


(setf start (list 0 0 0)) 
(setf goal (list 0 -50 160)) 


;Polygons 

(setf b1 (list v11 vi2 v13 v14)) 
(setf b2 (list v21 v22 v23)) 
(setf b3 dist v31 v32 v33 v34)) 


(setf world (list b1 b2 b3)) 


C. TANGENTS.CL 


;Find all tangents from the start to all polygons in the world 


(defmethod find-start-tangents ((replanner replannerclass)) 
(with-slots (start goal world start-tangents) replanner 
(do* ((n 0 (1+ n))) 
((= n (length world))) 
(setf start-tangents 
(append start-tangents (list 
(list (tangent-from-start-polygon start (nth n world) ccw) 
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(tangent-from-start-polygon start (nth n world) cw)))))) 
(setf start-tangents 
(append start-tangents 
(list (list (list start goal) (list start goal))))))) 


(defun tangent-from-start-polygon (point polygon mode) 
(do* ((vertex (first polygon) (if (= minus-orientation mode) 
plus-next minus-next)) 
(minus-next (next vertex polygon (neg mode)) 
(next vertex polygon (neg mode))) 
(plus-next (next vertex polygon mode) 
(next vertex polygon mode)) 
(minus-orientation (orientation point vertex minus-next) 
(orientation point vertex minus-next)) 
(plus-orientation (orientation point vertex plus-next) 
(orientation point vertex plus-next))) 
((and 
(= minus-orientation mode) 
(/= plus-orientation (neg mode))) (list point vertex)))) 


RSESSSHSSHSHESHOSHHEEHSHHSEHEHSEHHHEH HEHEHE HSHSHSHSHEFSSEHSHSHEHESEHESHEHHEHHHSHSEHHHEHHESESHHHSHFSEHSEHSHESH 


FPIFIPDIPP—IIPIPIIFIPPIIVIFVIPIIIFIIPPIHPIFFIPIAIIPFIAIIIIIFIIDHIFPIIIIMPIVAIIIG 


-Find all tangents from the goal to all polygons in the world 
(defmethod find-goal-tangents ((replanner replannerclass) world goal) 
(with-slots (goal-tangents) replanner 
(do* ((n O (1+ n))) 
((=n (length world))) 
(setf goal-tangents 
(append goal-tangents (list 
(list (tangent-from-goal-polygon goal (nth n world) ccw) 
(tangent-from-goal-polygon goal (nth n world) cw)))))))) 


(defun tangent-from-goal-polygon (point polygon mode) 
(do* ((vertex (first polygon) (if (= minus-orientation mode) 
plus-next minus-next)) 
(minus-next (next vertex polygon (neg mode)) 
(next vertex polygon (neg mode))) 
(plus-next (next vertex polygon mode) 
(next vertex polygon mode)) 
(minus-orientation (orientation point vertex minus-next) 
(orientation point vertex minus-next)) 
(plus-orientation (orientation point vertex plus-next) 
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(orientation point vertex plus-next))) 
((and 
(= minus-orientation mode) 
(/= plus-orientation (neg mode))) (list vertex point)))) 


SHS SHSHSEH HSH ASHEHESHEHSHSSEHHSHSHEHHFSHHSHSHEHSSHHEHHEHSCHSSHEHESHHHEHSHSSHESEHHEHEHEHEHHSHHOFESHR EBSD 


PIF FFF FTF IF AFIT IFPI III III III IIDIVIFIVDIIIIIFIIIIIFIIIIIIIFIIIIIIIIFIFD 


;Find all of the tangents from one polygon to all other polygons in the world 
(defmethod find-all-polygon-tangents ((replanner replannerclass)) 
(with-slots (world polygon-tangents) replanner 
(do* ((n Q (1+ n)) 

(from-poly (first world) (nth n world)) 

(rest-of-world (rest world) (remove-nth n world))) 

((=n (length world))) 

(setf polygon-tangents 
(append polygon-tangents (list 
(find-one-polygons-tangents from-poly rest-of-world))))))) 


(defun find-one-polygons-tangents (polygon world) 
(let* ((poly-index (first polygon))) 

(do* ((target-polygon (first world) (first remaining-world)) 
(remaining-world (rest world) (rest remaining-world)) 
(tangent-list 

(list (find-tangents-of-all-modes polygon target-polygon)) 
(append tangent-list 
(list (find-tangents-of-all-modes polygon target-polygon))))) 
((= 0 (length remaining-world)) tangent-list)))) 


(defun find-tangents-of-all-modes (polyl poly2) 

(list (tangent-between-two-polygons polyl poly2 ccw ccw) 
(tangent-between-two-polygons poly1 poly2 cw ccw) 
(tangent-between-two-polygons poly1 poly2 ccw cw) 
(tangent-between-two-polygons poly1 poly2 cw cw))) 


(defun tangent-between-two-polygons (B1 B2 ul u2) 
(let* ((v1 (first B1)) 
(v2 (first B2)) 
(flag1 nil) 
(flag2 nil) 
(minus-u2-orientation nil) 
(u2-orientation nil) 
(minus-ul-orientation nil) 
(ul-orientation nil)) 
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(loop 
(setf minus-u2-orientation (orientation v1 v2 (next v2 B2 (neg u2)))) 
(setf u2-orientation (orientation v1 v2 (next v2 B2 u2))) 
(setf minus-ul-orientation (orientation v2 v1 (next v1 B1 (neg ul)))) 
(setf ul-orientation (orientation v2 v1 (next v1 B1 ul))) 
Gif (= u2 minus-u2-orientation) 
(Gif (/= (neg u2) u2-orientation) (setf flag2 t) (setf v2 (next v2 B2 u2))) 
(setf v2 (next v2 B2 (neg u2)))) 
Gf (= ul-orientation (neg u1)) 
(if (/= ul minus-ul-orientation) (setf flag1 t) (setf v1 (next v1 B1 (neg ul)))) 
(setf v1 (next v1 B1 ul))) 
(if (and flag! flag2) (return (list v1 v2)))))) 


D. VISIBLE-POLYGONS.CL 


(defmethod find-visible-polygon-tangents ((replanner replannerclass)) 
(with-slots (world polygon-tangents) replanner 
(let* ((list-length (1- (ength polygon-tangents)))) 
(do* ((n 0 (1+ n)) 
(new-polygon-tangents 
(list (single-polygon-visible-tangents (first polygon-tangents) 
(remove-nth n world))) 
(append new-polygon-tangents (list 
(single-polygon-visible-tangents (nth n polygon-tangents) 
(remove-nth n world)))))) 
((=n list-length) 
(setf polygon-tangents new-polygon-tangents)))))) 


(defun single-polygon-visible-tangents (polygon-tangent-list check-world) 
(let* ((list-length (1- (ength polygon-tangent-list)))) 
(do* ((n 0 (1+ n)) 

(visible-list 

(list (visible-line-segments (first polygon-tangent-list) 
check-world)) 
(append visible-list (list 
(visible-line-segments (nth n polygon-tangent-list) 
check-world))))) 
((=n list-length) visible-list)))) 


(defun visible-line-segments (tangent-set check-world) 


(do* ((n 0 (1+ n)) 
(visible-lines (if (test-if-invisible (first tangent-set) check-world) 
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(list invisible-tangent) 
(list (first tangent-set))) 
(if (test-if-invisible (nth n tangent-set) check-world) 
(append visible-lines (list invisible-tangent)) 
(append visible-lines (list (nth n tangent-set)))))) 
((=n 3) visible-lines))) 


‘returns t if invisible 
(defun test-if-invisible (tangent check-world) 
(let* ((list-length (1- (length check-world)))) 
(do* ((n O (1+ n)) 
(check-poly (first check-world) (nth n check-world)) 
(invisible-flag (check-line-against-poly tangent check-poly) 
(check-line-against-poly tangent check-poly))) 
((or invisible-flag (= n list-length)) invisible-flag)))) 


‘returns t if invisible 
(defun check-line-against-poly (tangent check-poly) 
(do* ((n O (1+ n)) 

(segment (list (first check-poly) 

(second check-poly)) 

(list (nth n check-poly) 
(next (nth n check-poly) 
check-poly ccw))) 

(crossing-flag (segment-crossing-test tangent segment) 

(segment-crossing-test tangent segment)) 
(invisible-flag Gf (= crossing-flag -1) nil 

(if (= crossing-flag 1) t 
(if G@nvisible-end-point-test 
tangent segment 
(next (nth n check-poly) check-poly cw)) 
t nil))) 
(if (= crossing-flag -1) nil 
Gif (= crossing-flag 1) t 
(if (¢nvisible-end-point-test 
tangent segment 
(next (nth n check-poly) check-poly cw)) 
t nil))))) 
((or invisible-flag (= n (1- (length check-poly)))) invisible-flag))) 


(defun invisible-end-point-test (tangent segment previous-point) 


Gf (and (= 0 (segment-crossing-test tangent segment)) 
(= 0 (segment-crossing-test tangent (list previous-point 
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(first segment))))) 
(let* ((previous-orientation (orientation (first tangent) 
(second tangent) 
previous-point)) 
(next-orientation (orientation (first tangent) 
(second tangent) 
(second segment)))) 
(if (or (and (>= previous-orientation 0) 
(>= next-orientation Q)) 
(and (<= previous-orientation Q) 
(<= next-orientation 0))) 
nil t)) 
nil)) 


E. VISIBLE-TANGENTS.CL 


(defmethod find-visible-first-tangents ((replanner replannerclass) tangent-list) 
(let* ((list-length (1- (length tangent-list)))) 
(do* (n 0 (1+ n)) 

(check-list (rest tangent-list) (temove-nth n tangent-list)) 

(new-visible-tangents 
(list (first-point-visible-tangents (first tangent-list) check-list)) 
(append new-visible-tangents 

(list (first-point-visible-tangents (nth n tangent-list) 
check-list))))) 
((=n list-length) new-visible-tangents)))) 


(defmethod find-visible-last-tangents ((replanner replannerclass) tangent-list) 
(let* ((list-length (1- (ength tangent-list)))) 
(do* ((n 0 (1+ n)) 

(check-list (rest tangent-list) (remove-nth n tangent-list)) 

(new-visible-tangents 
(list (last-point-visible-tangents (first tangent-list) check-list)) 
(append new-visible-tangents 

(list (last-point-visible-tangents (nth n tangent-list) 
check-list))))) 
((=n list-length) new-visible-tangents)))) 


(defun first-point-visible-tangents (tangent-set check-list) 


(do* ((n 0 (1+ n)) 
(visible-lines (if (test-first-invisible (first tangent-set) check-list) 
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(list invisible-tangent) 
(list (first tangent-set))) 
(if (test-first-invisible (second tangent-set) check-list) 
(append visible-lines (list invisible-tangent)) 
(append visible-lines (list (nth n tangent-set)))))) 
((=n 1) visible-lines))) 


(defun last-point-visible-tangents (tangent-set check-list) 
(do* ((n 0 (1+ n)) 

(visible-lines (if (test-last-invisible (first tangent-set) check-list) 
(list invisible-tangent) 
(list (first tangent-set))) 

(if (test-last-invisible (second tangent-set) check-list) 

(append visible-lines (list invisible-tangent)) 
(append visible-lines (list (nth n tangent-set)))))) 

((=n 1) visible-lines))) 


(defun test-first-invisible (tangent check-list) ;returns t if invisible 
(let* ((list-length (1- dength check-list)))) 
(do* ((n 0 (14+ n)) 

(segment (list (second (first (first check-list))) 

(second (second (first check-list)))) 
(list (second (first (nth n check-list))) 

(second (second (nth n check-list))))) 

(invisible-flag Gif (< 0 (segment-crossing-test tangent segment)) t nil) 
(if (<< 0 (segment-crossing-test tangent segment)) 

t invisible-flag))) 
((or invisible-flag 
(= n list-length)) invisible-flag)))) 


(defun test-last-invisible (tangent check-list) ;returns t if invisible 
(let* ((list-length (1- (length check-list)))) 
(do* ((n 0 (1+ n)) 

(segment (list (first (first (first check-list))) 

(first (second (first check-list)))) 
(list (first (first (nth n check-list))) 

(first (second (nth n check-list))))) 

(invisible-flag (if (< 0 (segment-crossing-test tangent segment)) t nil) 
(if (< 0 (segment-crossing-test tangent segment)) 

t invisible-flag))) 
((or invisible-flag 
(=n list-length)) invisible-flag)))) 
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F. DYKSTRA-SEARCH.CL 


(defstruct dir-poly 
mark 
cost 
land 
previous 
leave 
area) 


sHigh level function which implements the dykstra search. 
(defmethod find-shortest-path ((replanner replannerclass)) 
(build-dykstra-table replanner) ;method to build the table slot 
(dykstra-search replanner) 
(return-shortest-path replanner)) 


‘Builds a table which contains state information for each of the directed 
polygons in the world. 
(defmethod build-dykstra-table ((replanner replannerclass)) 
(with-slots (start world dykstra-table) replanner 
(setf dykstra-table (list (make-dir-poly :mark 1 
:cost 0 
:land start 
‘previous start 
:area Q))) 
(do (n 0 (1+ n))) 
((=n (* 2 (length world))) (setf dykstra-table (append dykstra-table 
(list (make-dir-poly 
:mark 0 
:cost infinity 
sleave -1 
:area 0))))) 
(setf dykstra-table (append dykstra-table (list (make-dir-poly 
:mark 0 
:cost infinity 
‘leave infinity 


-area 0))))))) 


(defmethod dykstra-search ((replanner replannerclass)) 


(with-slots (dykstra-table) replanner 
(do* ((z-index (go-from-z replanner 0)(go-from-z replanner z-index))) 


((equal z-index (1- (length dykstra-table))))))) 


(defmethod go-from-z ((replanner replannerclass) z-index) 
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(dotimes (n (polygon-nodes replanner z-index) (find-next-z replanner)) 
(mark-landings replanner z-index n))) 


(defmethod find-next-z ((replanner replannerclass)) 
(with-slots (dykstra-table) replanner 
(let* ((min-cost-index Q) 
(min-cost infinity)) 
(do* ((index 1 (1+ index))) 
((= index (length dykstra-table)) 
(setf (dir-poly-mark (nth min-cost-index dykstra-table)) 1) 
min-cost-index) 
(if (and (/= (dir-poly-mark (nth index dykstra-table)) 1) 
(< (dir-poly-cost (nth index dykstra-table)) min-cost)) 
(and (setf min-cost (dir-poly-cost (nth index dykstra-table))) 
(setf min-cost-index index))))))) 


(defmethod mark-landings ((replanner replannerclass) z-index vertice-index) 
(with-slots (dykstra-table) replanner 
(let* ((leaving-vertice (which-vertice replanner z-index vertice-index)) 
(landing-list (return-landings replanner z-index leaving-vertice)) 
(vertice-cost (boundary-distance replanner 
z-index 
(dir-poly-land (nth z-index dykstra-table)) 
leaving-vertice)) 
(landing-vertice (dir-poly-land (nth z-index dykstra-table))) 
(z-area (dir-poly-area (nth z-index dykstra-table)))) 
(dotimes (n (length landing-list) ()) 
(let* 
((z-prime (first (nth n landing-list))) 
(z-prime-index (second (nth n landing-list))) 
(z-prime-area (dir-poly-area (nth z-prime-index dykstra-table))) 
(w (+ (dir-poly-cost (nth z-index dykstra-table)) 
_-vertice-cost 
(distance (rest leaving-vertice) (rest z-prime)))) 
(a0 (+ z-area (big-d (rest landing-vertice) (rest leaving-vertice)) 
(big-d (rest leaving-vertice) (rest z-prime))))) 
(if (or (= (dir-poly-cost (nth z-prime-index dykstra-table)) infinity) 
(and (= (sign (mode replanner z-prime-index)) 
(sign (+ z-prime-area 
(big-d 
(rest (dir-poly-land (nth z-prime-index dykstra-table))) 
(rest z-prime)) 
(neg a0)))) 
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(<(+ w 
(boundary-distance replanner 
Z-prime-index 
Z-prime 
(dir-poly-land (nth z-prime-index dykstra-table)))) 
(dir-poly-cost (nth z-prime-index dykstra-table)))) 
(and (/= (sign (mode replanner z-prime-index)) 
(sign (+ z-prime-area 
(big-d 
(rest (dir-poly-land (nth z-prime-index dykstra-table))) 
(rest z-prime)) 
(neg aQ)))) 
(<(-w 
(boundary-distance replanner 
Z-prime-index 
(dir-poly-land (nth z-prime-index dykstra-table)) 
Z-prime)) 


(dir-poly-cost (nth z-prime-index dykstra-table))))) 
(and 
(setf (dir-poly-cost (nth z-prime-index dykstra-table)) w) 
(setf (dir-poly-land (nth z-prime-index dykstra-table)) z-prime) 
(setf (dir-poly-previous (nth z-prime-index dykstra-table)) z-index) 
(setf (dir-poly-leave (nth z-prime-index dykstra-table)) leaving-vertice) 
(setf (dir-poly-area (nth z-prime-index dykstra-table)) aQ)))))))) 


(defmethod which-vertice ((replanner replannerclass) table-index vertice-index) 
(with-slots (start goal dykstra-table) replanner 
(if (= table-index 0) start 
(if (= table-index (1- (length dykstra-table))) goal 
(let* ((new-vertex (dir-poly-land (nth table-index dykstra-table)))) 
(dotimes (n vertice-index new-vertex) 


(setf new-vertex (jump-one replanner table-index new-vertex)))))))) 


(defmethod boundary-distance ((replanner replannerclass) table-index s-point vertice) 
(with-slots (dykstra-table) replanner 


(if (or (= table-index 0) 
(= table-index (1- (ength dykstra-table))) 
(equal vertice s-point)) 0 
(do* ((old-vertex s-point 
new-vertex ) 
(new-vertex (jump-one replanner table-index s-point) 
(jump-one replanner table-index new-vertex)) 
(distance-total (distance (rest old-vertex) (rest new-vertex)) 
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(+ distance-total 
(distance (rest old-vertex) (rest new-vertex))))) 
((equal new-vertex vertice) distance-total))))) 


(defmethod return-landings ((replanner replannerclass) z-index vertice) 
(with-slots (polygon-tangents) replanner 
(let* ((polygon (if (= 0 z-index) 0 (floor (/ (- z-index 1) 2)))) 
(landing-list (if (> z-index 0) (check-one-poly vertice polygon 
(nth polygon polygon-tangents)) 
(check-start-leaves replanner))) 
(goal-landings (check-goal-landings replanner vertice polygon))) 
(if (equal goal-landings nil) landing-list 
(append landing-list goal-landings))))) 


(defmethod check-goal-landings ((replanner replannerclass) vertex polygon) 
(with-slots (goal-tangents dykstra-table) replanner 
(let* ((goall (if (equal vertex (first (first (nth polygon goal-tangents)))) 
(list (second (first (nth polygon goal-tangents))) 
(1- (length dykstra-table))) nil)) 
(goal2 (if (equal vertex (first (second (nth polygon goal-tangents)))) 
(list (second (second (nth polygon goal-tangents))) 
| (1- dength dykstra-table))) nil))) 
(if (equal goal1 nil) 
(Gif (equal goal2 nil) nil (list goal2)) 
(if (equal goal2 nil) (list goal1) (list goall goal2)))))) 


(defun check-one-poly (vertex polygon tangent-list) 
(let* ((partial-list nil)) 
(dotimes (n (length tangent-list) partial-list) 
(if (equal vertex (first (nth 0 (nth n tangent-list)))) 
(setf partial-list (append partial-list 
(list (list 
(second (nth 0 (nth n tangent-list))) 
(Gif (>= n polygon) (- (* (+ n 2) 2) 1) 
(- (* (+n 1) 2) 1))))))) 
(if (equal vertex (first (nth 1 (nth n tangent-list)))) 
(setf partial-list (append partial-list 
(list (list 
(second (nth 1 (nth n tangent-list))) 
(if (>= n polygon) (- (* (4.n 2) 2) 1) 
(- (* (+n 1) 2) 1)))))) 
(if (equal vertex (first (nth 2 (nth n tangent-list)))) 
(setf partial-list (append partial-list 
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(list (list 
(second (nth 2 (nth n tangent-list))) 
(if (>= n polygon) (* (+ n 2) 2) 
(* (+n 1) 2))))))) 
(if (equal vertex (first (nth 3 (nth n tangent-list)))) 
(setf partial-list (append partial-list 
(list (list 
(second (nth 3 (nth n tangent-list))) 
(if (>= n polygon) (* (+ n 2) 2) 
(* (+n 1) 2))))))))) 


(defmethod check-start-leaves ((replanner replannerclass)) 
(with-slots (start start-tangents) replanner 
(let* ((partial-list ())) 
(dotimes (n (length start-tangents) partial-list) 
(if (equal start (first (first (nth n start-tangents)))) 
(setf partial-list (append partial-list (list (list 
(second (first (nth n start-tangents))) 
(- * +n 1) 2) 1)))))) 
(if (equal start (first (second (nth n start-tangents)))) 
(setf partial-list (append partial-list (list (list 
(second (second (nth n start-tangents))) 
(* (+n 1) 2)))))))) 


(defmethod polygon-nodes ((replanner replannerclass) table-index) 
(with-slots (dykstra-table) replanner 
(if (or (= table-index 0) (= table-index (1- (length dykstra-table)))) 1 
(length (find-polygon replanner table-index))))) 


(defmethod mode ((replanner replannerclass) table-index) 
(with-slots (dykstra-table) replanner 
(if (or (= O table-index) 
(= table-index (1- (length dykstra-table)))) 
0 Gf (even table-index) cw ccw)))) 


(defmethod find-polygon ((replanner replannerclass) table-index) 
(with-slots (start goal world dykstra-table) replanner 
(if (= O table-index) start 
(if (= table-index (1- (length dykstra-table))) goal 
(nth (floor (/ (- table-index 1) 2)) world))))) 


(defmethod jump-one ((replanner replannerclass) table-index vertex) 
(next vertex (find-polygon replanner table-index) (mode replanner table-index))) 
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(defmethod return-shortest-path ((replanner replannerclass)) 
(with-slots (goal world dykstra-table) replanner 
(let* ((path-list (list goal))) 
(do* ((cur-index (+ (* (length world) 2) 1) 
(dir-poly-previous (nth cur-index dykstra-table))) 
(previous-index (dir-poly-previous (nth cur-index dykstra-table)) 
(dir-poly-previous (nth cur-index dykstra-table)))) 
((= cur-index 0) (reverse path-list)) 
Gif (equal (dir-poly-leave (nth cur-index dykstra-table)) 
(dir-poly-land (nth previous-index dykstra-table))) 
(setf path-list (append path-list (list 
(dir-poly-leave (nth cur-index dykstra-table))))) 
(setf path-list (append path-list 
(traverse-perimeter replanner 
previous-index 
(dir-poly-leave (nth cur-index dykstra-table)) 
(dir-poly-land (nth previous-index dykstra-table)))))))))) 


(defmethod traverse-perimeter ((replanner replannerclass) index leave land) 
(with-slots (world dykstra-table) replanner 
(do* ((old-vertex leave new-vertex) 
(new-vertex (next old-vertex (find-polygon replanner index) (neg (mode replanner 
index))) 
(next old-vertex (find-polygon replanner index) (neg (mode replanner index)))) 
(vertex-list (list new-vertex leave) (cons new-vertex vertex-list))) 
((equal new-vertex land) (reverse vertex-list))))) 


G. REPLAN-FUNCTIONS.CL 
(defconstant limit 0.000001) 


(defun sign (x) 
(if (< x (neg limit)) -1 
(if (> x limit) 1 0))) 


(defun orientation(p1 p2 p3) 
(let ((area (* 0.5 ( (* ( (Second p2) (second p1)) 
(- (third p3) (third p1))) 
(* (- (second p3) (second p1)) 
(- (third p2) (third p1))))))) 
(if (> area 0.0) ccw 
(if (< area 0.0) cw Q)))) 
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(defun next(vertex polygon mode) 
(let (Gi (first vertex))) 
(if (> mode 0) 
(if (= i dength polygon)) (first polygon) 
(nth 1 polygon)) 
(previous vertex polygon)))) 


(defun previous(vertex polygon) 
(let ((i (first vertex))) 
(if (=i 1) (nth (1- (length polygon)) polygon) 
(nth (- 1 2) polygon)))) 


(defun from-n-on (n list) ;elements numbered 0 1 2 ... 
(cond ((>= n (length list)) Q) 
(t (cons (nth n list) (from-n-on (1+ n) list))))) 


(defun remove-nth (n list) ;elements numbered 0 1 2 ... 
(append (firstj n list) (from-n-on (1+ n) list))) 


(defun make-line-segment(v1 v2) 
(list v1 v2)) 


(defun segment-crossing-test(si s2) 

(let* ((01 (orientation (first s1) (second s1) (first s2))) 
(02 (orientation (first s1) (second s1) (second s2))) 
(03 (orientation (first s2) (second s2) (first s1))) 
(04 (orientation (first s2) (second s2) (second s1)))) 

(Gif (crossing-test sl s2 ol 02 03 04) 1 

(if (touching-test s1 s2 01 02 03 04) 0 

(if (overlap-test s1 s2 ol 02 03 04) 0 -1))))) 


(defun crossing-test(sl s2 01 02 03 04) 
(if (and (/= 01 0) (/= 02 0) (= ol 02) 
(/= 03 0) (/= 04 0) (= 03 04)) t Q)) 


(defun touching-test(s1 s2 01 02 03 04) 
Gf (or (and (or (= 01 0) (= 02 0)) (and (/= 03 0) (/= 04 0) (/= 03 04))) 
(and (or (= 03 0) (= 04 0)) (and (/= o1 0) (/= 02 0) (= o1 02))) 
(and (and (or (= 01 0) (= 02 0)) (= 01 02)) (and (or (= 03 0) 
(= 04 0)) (= 03 04)))) t Q)) 


(defun overlap-test(sl s2 01 02 03 04) 
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(if (= (and ol 02 03 04) 0) 
(let* ((f1 (linearize (first s1) s1)) 
(f2 (linearize (second s1) s1)) 
(f3 (linearize (first s2) s1)) 
(f4 (linearize (second s2) s1))) 
(if (or (and (< f3 0.0) (< £4 0.0)) 
(and (> f3 f2) (> f4 f2))) nil 
(if (or (and (<= f3 f2) (>= f3 f1)) 
(and (<= f4 f2) (>= f4 f1))) t O))))) 


(defun linearize(v 1) 
(let* ((f1 (* (- (second v) (second (first 1))) 
(- (second (second 1)) (second (first 1))))) 
(f2 (* (- (third v) (third (first 1))) 
(- (third (second 1)) (third (first 1)))))) 
(+ fl £2))) 


(defun big-d (v1 v2) 
(* 0.5 (- (* (first v1) (second v2)) (* (first v2) (second v1))))) 


(defun even (number) 
(if (= 0 (mod number 2)) t nil)) 


H. WIND-TRI.CL 
(defconstant two-pi 6.283185307179586) ;pi multiplied by two 


(defconstant pi-two 1.5707963267948966) ;pi divided by two 


(defun radian-true-course (from-point to-point) 
(positive-radians (atan (- (second to-point) (second from-point)) 
(- (first to-point) (first from-point))))) 


(defun positive-radians (radians) 
(if (< radians 0) (+ radians two-pi) radians)); 


(defun calculate-heading (speed course current) 
(cond ((zerop speed) course) 
(t (let* ((A (- course (second current))) 
(b (first current)) 
(a speed) 
(B (asin (/ (* b (sin A)) a)))) 


213 


(+ course B))))) 
(defun square (x) (* x x)) 
(defun deg-to-rad (deg) (* deg (/ pi 180))) 
(defun rad-to-deg (rad) (* rad (/ 180 pi))) 


(defun distance (from-point to-point) 
(let*((x-delta (- (first to-point) 
(first from-point))) 
(y-delta (- (second to-point) 
(second from-point)))) 
(sqrt (+ (square x-delta) (square y-delta))))) 


(defun find-vector (from-point to-point) 
(list (distance from-point to-point) 
(radian-true-course from-point to-point))) 


(defun firstj (n list) ;improved version 
(cond ((or(zerop n)(null list)) nil) 
(t (cons (first list) (first] (1- n) (rest list)))))) 


(defun power (exponent base) 
(do* (Gi exponent (1- 1)) 
(result base (* result base))) 
((=1 1) result))) 


(defun neg (number) 
(* -1 number)) 


(defun angle-trans (heading) 
(if (> heading pi) (- heading two-pi) heading)) 
snormalizes alpha to between pi and -pi 


(defun fee (alpha) 
(if (> alpha pi) (fee(- alpha two-pi)) 


(if (<= alpha (neg pi)) (fee(+ alpha two-pi)) alpha))) 
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APPENDIX E: Tattletale Source Code (TxBASIC) 


A. TATTLETALE CODE 


Written by Walter Schubert for the towfish experiment. 


3 SLEEP 0 
4 SLEEP 2000 
5 PRINT “ATA” 
6 SLEEP 3000 
10 A=16384 
20 B=A 
30 BURST B,8,2 
40 IF B < (A+128) GOTO 30 
50 OFFLD A,A+127,100,C 
60 GOTO 20 
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